Skip to content
Snippets Groups Projects
Commit cafcc216 authored by Projekt Robbie's avatar Projekt Robbie
Browse files

map_manager without laser scans

parent ac61d316
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
homer_map_manager/src/Managers/MapManager.cpp
+ 122
− 122
View file @ cafcc216
......@@ -21,18 +21,18 @@ MapManager::MapManager(ros::NodeHandle *nh) {
m_MapVisibility[m_laser_layers[i]] = true;
}
try {
m_TransformListener.waitForTransform("/base_link", "/laser",
ros::Time(0), ros::Duration(3));
m_TransformListener.lookupTransform("/base_link", "/laser",
ros::Time(0), m_sick_transform);
m_TransformListener.lookupTransform("/base_link", "/hokuyo_front",
ros::Time(0), m_hokuyo_transform);
m_got_transform = true;
} catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
m_got_transform = false;
}
//try {
//m_TransformListener.waitForTransform("/base_link", "/laser",
//ros::Time(0), ros::Duration(3));
//m_TransformListener.lookupTransform("/base_link", "/laser",
//ros::Time(0), m_sick_transform);
//m_TransformListener.lookupTransform("/base_link", "/hokuyo_front",
//ros::Time(0), m_hokuyo_transform);
//m_got_transform = true;
//} catch (tf::TransformException ex) {
//ROS_ERROR("%s", ex.what());
//m_got_transform = false;
//}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
......@@ -108,116 +108,116 @@ void MapManager::sendMergedMap() {
}
// bake Lasers
try {
int data;
if (m_got_transform) {
for (int j = 0; j < m_laser_layers.size(); j++) {
if (m_laserLayers.find(m_laser_layers[j]) !=
m_laserLayers.end() &&
m_MapVisibility[m_laser_layers[j]]) {
if (m_laser_layers[j] ==
homer_mapnav_msgs::MapLayers::SICK_LAYER) {
data = homer_mapnav_msgs::ModifyMap::BLOCKED;
} else if (m_laser_layers[j] ==
homer_mapnav_msgs::MapLayers::HOKUYO_BACK) {
data = homer_mapnav_msgs::ModifyMap::HOKUYO;
} else if (m_laser_layers[j] ==
homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) {
data = homer_mapnav_msgs::ModifyMap::HOKUYO;
}
tf::StampedTransform pose_transform;
m_TransformListener.waitForTransform(
"/map", "/base_link",
m_laserLayers[m_laser_layers[j]]->header.stamp,
ros::Duration(0.09));
m_TransformListener.lookupTransform(
"/map", "/base_link",
m_laserLayers[m_laser_layers[j]]->header.stamp,
pose_transform);
// pose_transform.setTransform(Transform);
for (int i = 0;
i < m_laserLayers[m_laser_layers[j]]->ranges.size();
i++) {
if (m_laserLayers[m_laser_layers[j]]->ranges[i] <
m_laserLayers[m_laser_layers[j]]->range_max &&
m_laserLayers[m_laser_layers[j]]->ranges[i] >
m_laserLayers[m_laser_layers[j]]->range_min) {
float alpha =
m_laserLayers[m_laser_layers[j]]->angle_min +
i *
m_laserLayers[m_laser_layers[j]]
->angle_increment;
geometry_msgs::Point point;
tf::Vector3 pin;
tf::Vector3 pout;
pin.setX(
cos(alpha) *
m_laserLayers[m_laser_layers[j]]->ranges[i]);
pin.setY(
sin(alpha) *
m_laserLayers[m_laser_layers[j]]->ranges[i]);
pin.setZ(0);
if (m_laser_layers[j] ==
homer_mapnav_msgs::MapLayers::SICK_LAYER) {
pout = pose_transform * m_sick_transform * pin;
} else if (m_laser_layers[j] ==
homer_mapnav_msgs::MapLayers::
HOKUYO_FRONT) {
pout =
pose_transform * m_hokuyo_transform * pin;
}
point.x = pout.x();
point.y = pout.y();
point.z = 0;
Eigen::Vector2i map_point = map_tools::toMapCoords(
point,
m_MapLayers
[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
->info.origin,
m_MapLayers
[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
->info.resolution);
k = map_point.y() *
m_MapLayers[homer_mapnav_msgs::MapLayers::
SLAM_LAYER]
->info.width +
map_point.x();
if (k < 0 ||
k > m_MapLayers[homer_mapnav_msgs::MapLayers::
SLAM_LAYER]
->data.size()) {
continue;
} else {
mergedMap.data[k] = data;
}
}
}
}
}
} else {
try {
if (m_TransformListener.waitForTransform("/base_link", "/laser",
ros::Time(0),
ros::Duration(0.1))) {
m_TransformListener.lookupTransform(
"/base_link", "/laser", ros::Time(0), m_sick_transform);
m_TransformListener.lookupTransform(
"/base_link", "/hokuyo_front", ros::Time(0),
m_hokuyo_transform);
m_got_transform = true;
}
} catch (tf::TransformException ex) {
ROS_ERROR("%s", ex.what());
m_got_transform = false;
}
}
} catch (tf::TransformException ex) {
ROS_ERROR_STREAM(ex.what());
}
//try {
//int data;
//if (m_got_transform) {
//for (int j = 0; j < m_laser_layers.size(); j++) {
//if (m_laserLayers.find(m_laser_layers[j]) !=
//m_laserLayers.end() &&
//m_MapVisibility[m_laser_layers[j]]) {
//if (m_laser_layers[j] ==
//homer_mapnav_msgs::MapLayers::SICK_LAYER) {
//data = homer_mapnav_msgs::ModifyMap::BLOCKED;
//} else if (m_laser_layers[j] ==
//homer_mapnav_msgs::MapLayers::HOKUYO_BACK) {
//data = homer_mapnav_msgs::ModifyMap::HOKUYO;
//} else if (m_laser_layers[j] ==
//homer_mapnav_msgs::MapLayers::HOKUYO_FRONT) {
//data = homer_mapnav_msgs::ModifyMap::HOKUYO;
//}
//tf::StampedTransform pose_transform;
//m_TransformListener.waitForTransform(
//"/map", "/base_link",
//m_laserLayers[m_laser_layers[j]]->header.stamp,
//ros::Duration(0.09));
//m_TransformListener.lookupTransform(
//"/map", "/base_link",
//m_laserLayers[m_laser_layers[j]]->header.stamp,
//pose_transform);
//// pose_transform.setTransform(Transform);
//for (int i = 0;
//i < m_laserLayers[m_laser_layers[j]]->ranges.size();
//i++) {
//if (m_laserLayers[m_laser_layers[j]]->ranges[i] <
//m_laserLayers[m_laser_layers[j]]->range_max &&
//m_laserLayers[m_laser_layers[j]]->ranges[i] >
//m_laserLayers[m_laser_layers[j]]->range_min) {
//float alpha =
//m_laserLayers[m_laser_layers[j]]->angle_min +
//i *
//m_laserLayers[m_laser_layers[j]]
//->angle_increment;
//geometry_msgs::Point point;
//tf::Vector3 pin;
//tf::Vector3 pout;
//pin.setX(
//cos(alpha) *
//m_laserLayers[m_laser_layers[j]]->ranges[i]);
//pin.setY(
//sin(alpha) *
//m_laserLayers[m_laser_layers[j]]->ranges[i]);
//pin.setZ(0);
//if (m_laser_layers[j] ==
//homer_mapnav_msgs::MapLayers::SICK_LAYER) {
//pout = pose_transform * m_sick_transform * pin;
//} else if (m_laser_layers[j] ==
//homer_mapnav_msgs::MapLayers::
//HOKUYO_FRONT) {
//pout =
//pose_transform * m_hokuyo_transform * pin;
//}
//point.x = pout.x();
//point.y = pout.y();
//point.z = 0;
//Eigen::Vector2i map_point = map_tools::toMapCoords(
//point,
//m_MapLayers
//[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
//->info.origin,
//m_MapLayers
//[homer_mapnav_msgs::MapLayers::SLAM_LAYER]
//->info.resolution);
//k = map_point.y() *
//m_MapLayers[homer_mapnav_msgs::MapLayers::
//SLAM_LAYER]
//->info.width +
//map_point.x();
//if (k < 0 ||
//k > m_MapLayers[homer_mapnav_msgs::MapLayers::
//SLAM_LAYER]
//->data.size()) {
//continue;
//} else {
//mergedMap.data[k] = data;
//}
//}
//}
//}
//}
//} else {
//try {
//if (m_TransformListener.waitForTransform("/base_link", "/laser",
//ros::Time(0),
//ros::Duration(0.1))) {
//m_TransformListener.lookupTransform(
//"/base_link", "/laser", ros::Time(0), m_sick_transform);
//m_TransformListener.lookupTransform(
//"/base_link", "/hokuyo_front", ros::Time(0),
//m_hokuyo_transform);
//m_got_transform = true;
//}
//} catch (tf::TransformException ex) {
//ROS_ERROR("%s", ex.what());
//m_got_transform = false;
//}
//}
//} catch (tf::TransformException ex) {
//ROS_ERROR_STREAM(ex.what());
//}
mergedMap.header.stamp = ros::Time::now();
mergedMap.header.frame_id = "map";
......
homer_nav_libs/src/Explorer/Explorer.cpp
+ 75
− 26
View file @ cafcc216
......@@ -127,11 +127,21 @@ void Explorer::setStart(Eigen::Vector2i start) {
ROS_ERROR_STREAM("Occupancy map is missing.");
return;
}
if ((start.x() <= 1) || (start.y() <= 1) ||
(start.x() >= m_OccupancyMap->width() - 1) ||
(start.y() >= m_OccupancyMap->height() - 1)) {
ROS_ERROR_STREAM("Invalid position!");
return;
if (start.x() <= 1)
{
start.x() = 2;
}
if (start.y() <= 1)
{
start.y() = 2;
}
if (start.x() >= m_OccupancyMap->width() - 1)
{
start.x() = m_OccupancyMap->width() - 2;
}
if (start.y() >= m_OccupancyMap->height() - 1)
{
start.y() = m_OccupancyMap->height() -2;
}
computeWalkableMaps();
......@@ -156,11 +166,21 @@ Eigen::Vector2i Explorer::getNearestAccessibleTarget(Eigen::Vector2i target) {
ROS_ERROR("Occupancy map is missing.");
return target;
}
if ((target.x() <= 1) || (target.y() <= 1) ||
(target.x() >= m_OccupancyMap->width() - 1) ||
(target.y() >= m_OccupancyMap->height() - 1)) {
ROS_ERROR("Invalid position!");
return target;
if (target.x() <= 1)
{
target.x() = 2;
}
if (target.y() <= 1)
{
target.y() = 2;
}
if (target.x() >= m_OccupancyMap->width() - 1)
{
target.x() = m_OccupancyMap->width() - 2;
}
if (target.y() >= m_OccupancyMap->height() - 1)
{
target.y() = m_OccupancyMap->height() -2;
}
computeApproachableMaps();
computeWalkableMaps();
......@@ -204,11 +224,21 @@ Eigen::Vector2i Explorer::getNearestWalkablePoint(Eigen::Vector2i target) {
ROS_ERROR("Occupancy map is missing.");
return target;
}
if ((target.x() <= 1) || (target.y() <= 1) ||
(target.x() >= m_OccupancyMap->width() - 1) ||
(target.y() >= m_OccupancyMap->height() - 1)) {
ROS_ERROR("Invalid position!");
return target;
if (target.x() <= 1)
{
target.x() = 2;
}
if (target.y() <= 1)
{
target.y() = 2;
}
if (target.x() >= m_OccupancyMap->width() - 1)
{
target.x() = m_OccupancyMap->width() - 2;
}
if (target.y() >= m_OccupancyMap->height() - 1)
{
target.y() = m_OccupancyMap->height() -2;
}
computeWalkableMaps();
......@@ -243,11 +273,21 @@ void Explorer::setTarget(Eigen::Vector2i target) {
ROS_ERROR("Occupancy map is missing.");
return;
}
if ((target.x() <= 1) || (target.y() <= 1) ||
(target.x() >= m_OccupancyMap->width() - 1) ||
(target.y() >= m_OccupancyMap->height() - 1)) {
ROS_ERROR("Invalid position!");
return;
if (target.x() <= 1)
{
target.x() = 2;
}
if (target.y() <= 1)
{
target.y() = 2;
}
if (target.x() >= m_OccupancyMap->width() - 1)
{
target.x() = m_OccupancyMap->width() - 2;
}
if (target.y() >= m_OccupancyMap->height() - 1)
{
target.y() = m_OccupancyMap->height() -2;
}
computeApproachableMaps();
if (!isApproachable(target.x(), target.y())) {
......@@ -271,12 +311,21 @@ void Explorer::setTarget(Eigen::Vector2i target, int desiredDistance) {
return;
}
if (target.x() + desiredDistance <= 1 ||
target.x() - desiredDistance >= m_OccupancyMap->width() - 1 ||
target.y() + desiredDistance <= 1 ||
target.y() - desiredDistance >= m_OccupancyMap->height() - 1) {
ROS_ERROR("Invalid position");
return;
if (target.x() + desiredDistance <= 1)
{
target.x() = 2;
}
if (target.y() + desiredDistance <= 1)
{
target.y() = 2;
}
if (target.x() - desiredDistance >= m_OccupancyMap->width() - 1)
{
target.x() = m_OccupancyMap->width() - 2;
}
if (target.y() - desiredDistance >= m_OccupancyMap->height() - 1)
{
target.y() = m_OccupancyMap->height() -2;
}
computeApproachableMaps();
// TODO: check if region is approachable
......
homer_navigation/include/homer_navigation/homer_navigation_node.h
+ 9
− 4
View file @ cafcc216
......@@ -62,6 +62,7 @@ class HomerNavigationNode {
protected:
/** @brief Handles incoming messages. */
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg);
void ignoreLaserCallback(const std_msgs::String::ConstPtr& msg);
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& msg);
void laserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
void downlaserDataCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
......@@ -83,6 +84,7 @@ class HomerNavigationNode {
virtual void init();
void initNewTarget();
void processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg);
private:
/** @brief Start navigation to m_Target on last_map_data_ */
......@@ -219,6 +221,9 @@ class HomerNavigationNode {
geometry_msgs::Pose m_robot_last_pose;
std::map<std::string, sensor_msgs::LaserScan::ConstPtr> m_scan_map;
std::map<std::string, float> m_max_move_distances;
std::string m_ignore_scan;
/** time stamp of the last incoming laser scan */
ros::Time m_last_laser_time;
......@@ -244,9 +249,6 @@ class HomerNavigationNode {
float m_waypoint_sampling_threshold;
float m_max_move_distance;
float m_max_move_sick;
float m_max_move_down;
float m_max_move_depth;
/** if distance to nearest obstacle is below collision distance trigger
* collision avoidance */
......@@ -262,6 +264,9 @@ class HomerNavigationNode {
*/
bool m_check_path;
bool m_obstacle_on_path;
geometry_msgs::Point m_obstacle_position;
/** waypoints will only be checked for obstacles if they are closer than
* check_path_max_distance to robot */
float m_check_path_max_distance;
......@@ -299,7 +304,6 @@ class HomerNavigationNode {
bool m_path_reaches_target;
bool m_initial_path_reaches_target;
int m_last_calculations_failed;
int m_unknown_threshold;
/** last map data */
......@@ -321,6 +325,7 @@ class HomerNavigationNode {
ros::Subscriber m_refresh_param_sub;
ros::Subscriber m_max_move_depth_sub;
ros::Subscriber m_move_base_simple_goal_sub;
ros::Subscriber m_ignore_laser_sub;
// publishers
ros::Publisher m_cmd_vel_pub;
......
homer_navigation/src/homer_navigation_node.cpp
+ 116
− 66
View file @ cafcc216
......@@ -34,6 +34,8 @@ HomerNavigationNode::HomerNavigationNode() {
m_max_move_depth_sub = nh.subscribe<std_msgs::Float32>(
"/homer_navigation/max_depth_move_distance", 1,
&HomerNavigationNode::maxDepthMoveDistanceCallback, this);
m_ignore_laser_sub = nh.subscribe<std_msgs::String>("/homer_navigation/ignore_laser",1,&HomerNavigationNode::ignoreLaserCallback ,this);
m_cmd_vel_pub =
nh.advertise<geometry_msgs::Twist>("/robot_platform/cmd_vel", 3);
......@@ -142,9 +144,6 @@ void HomerNavigationNode::loadParameters() {
void HomerNavigationNode::init() {
m_explorer = 0;
m_max_move_sick = 40.0;
m_max_move_down = 40.0;
m_max_move_depth = 40.0;
m_robot_pose.position.x = 0.0;
m_robot_pose.position.y = 0.0;
m_robot_pose.position.z = 0.0;
......@@ -153,8 +152,14 @@ void HomerNavigationNode::init() {
m_avoided_collision = false;
m_act_speed = 0;
m_angular_avoidance = 0;
m_last_calculations_failed = 0;
m_last_check_path_time = ros::Time::now();
m_ignore_scan = "";
m_obstacle_on_path = false;
m_obstacle_position.x = 0;
m_obstacle_position.y = 0;
m_obstacle_position.z = 0;
nav_msgs::OccupancyGrid tmp_map;
tmp_map.header.frame_id = "/map";
tmp_map.info.resolution = 0;
......@@ -208,14 +213,16 @@ void HomerNavigationNode::setExplorerMap() {
// adding lasers to map
nav_msgs::OccupancyGrid temp_map = *m_map;
for (const auto& scan : m_scan_map) {
std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points(
scan.second, m_transform_listener, "/map");
for (const auto& point : scan_points) {
Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map);
int index = map_point.y() * m_map->info.width + map_point.x();
if (index > 0 && index < m_map->info.width * m_map->info.height) {
temp_map.data[index] = (int8_t)100;
if(m_ignore_scan.find(scan.second->header.frame_id) == std::string::npos){
std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points(
scan.second, m_transform_listener, "/map");
for (const auto& point : scan_points) {
Eigen::Vector2i map_point = map_tools::toMapCoords(point, m_map);
int index = map_point.y() * m_map->info.width + map_point.x();
if (index > 0 && index < m_map->info.width * m_map->info.height) {
temp_map.data[index] = (int8_t)100;
}
}
}
}
......@@ -240,16 +247,10 @@ void HomerNavigationNode::calculatePath() {
bool success;
m_pixel_path = m_explorer->getPath(success);
if (!success) {
ROS_WARN_STREAM("No path found for navigation");
m_last_calculations_failed++;
ROS_INFO_STREAM(
"last_calculation_failed: " << m_last_calculations_failed);
if (m_last_calculations_failed > 8) {
sendTargetUnreachableMsg(
homer_mapnav_msgs::TargetUnreachable::NO_PATH_FOUND);
}
ROS_WARN_STREAM("no path to target possible - drive to obstacle");
m_obstacle_on_path = true;
} else {
m_last_calculations_failed = 0;
m_obstacle_on_path = false;
std::vector<Eigen::Vector2i> waypoint_pixels =
m_explorer->sampleWaypointsFromPath(m_pixel_path,
m_waypoint_sampling_threshold);
......@@ -415,33 +416,35 @@ bool HomerNavigationNode::obstacleOnPath() {
m_last_check_path_time = ros::Time::now();
if (m_pixel_path.size() != 0) {
for (auto const& scan : m_scan_map) {
std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points(
scan.second, m_transform_listener, "/map");
for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) {
geometry_msgs::Point lp=
map_tools::fromMapCoords(m_pixel_path.at(i-1), m_map);
geometry_msgs::Point p =
map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
if (map_tools::distance(m_robot_pose.position, p) >
m_check_path_max_distance * 2) {
return false;
}
for(int k = 0; k < 4; k++)
{
geometry_msgs::Point t;
t.x =lp.x + (p.x - lp.x) * k/ 4.0;
t.y =lp.y + (p.y - lp.y) * k/ 4.0;
for(const auto &sp: scan_points)
if(m_ignore_scan.find(scan.second->header.frame_id) == std::string::npos){
std::vector<geometry_msgs::Point> scan_points;
scan_points = map_tools::laser_msg_to_points(
scan.second, m_transform_listener, "/map");
for (unsigned i = 1; i < m_pixel_path.size() - 1; i++) {
geometry_msgs::Point lp =
map_tools::fromMapCoords(m_pixel_path.at(i-1), m_map);
geometry_msgs::Point p =
map_tools::fromMapCoords(m_pixel_path.at(i), m_map);
if (map_tools::distance(m_robot_pose.position, p) >
m_check_path_max_distance * 2) {
return false;
}
for(int k = 0; k < 4; k++)
{
if (map_tools::distance(sp, t) <
m_AllowedObstacleDistance.first - m_map->info.resolution) {
if(map_tools::distance(m_robot_pose.position, sp)< m_check_path_max_distance)
{
ROS_INFO_STREAM("Found obstacle");
return true;
geometry_msgs::Point t;
t.x = lp.x + (p.x - lp.x) * k/ 4.0;
t.y = lp.y + (p.y - lp.y) * k/ 4.0;
for(const auto &sp: scan_points)
{
if (map_tools::distance(sp, t) <
m_AllowedObstacleDistance.first - m_map->info.resolution) {
if(map_tools::distance(m_robot_pose.position, sp) < m_check_path_max_distance)
{
m_obstacle_position = t;
ROS_INFO_STREAM("Found obstacle");
return true;
}
}
}
}
......@@ -462,9 +465,16 @@ void HomerNavigationNode::performNextMove() {
(ros::Time::now() - m_last_check_path_time) > ros::Duration(0.2)) {
if (obstacleOnPath()) {
if (m_seen_obstacle_before) {
stopRobot();
calculatePath();
return;
if(!m_obstacle_on_path)
{
stopRobot();
calculatePath();
return;
}
else
{
calculatePath();
}
}
m_seen_obstacle_before = true;
} else {
......@@ -571,11 +581,29 @@ void HomerNavigationNode::performNextMove() {
// linear speed calculation
if (m_avoided_collision) {
if (std::abs(angleToWaypoint) < 10) {
if (std::fabs(angleToWaypoint) < 10) {
m_avoided_collision = false;
}
} else if (abs(angle) > m_max_drive_angle) {
} else if (fabs(angle) > m_max_drive_angle) {
m_act_speed = 0.0;
} else if (m_obstacle_on_path && map_tools::distance(m_robot_pose.position, m_obstacle_position) < 1.0 )
{
m_act_speed = 0.0;
float angleToWaypoint2 = angleToPointDeg(m_obstacle_position);
if (angleToWaypoint2 < -180) {
angleToWaypoint2 += 360;
}
angle = deg2Rad(angleToWaypoint2);
if(std::fabs(angle) < m_min_turn_angle)
{
ROS_INFO_STREAM("angle = "<<angle);
m_avoided_collision = true;
m_initial_path_reaches_target = true;
m_stop_before_obstacle = true;
startNavigation();
return;
}
} else {
float obstacleMapDistance = 1;
for (int wpi = -1; wpi < std::min((int)m_waypoints.size(), (int)2);
......@@ -992,8 +1020,12 @@ void HomerNavigationNode::poseCallback(
}
void HomerNavigationNode::calcMaxMoveDist() {
m_max_move_distance =
std::min({m_max_move_sick, m_max_move_down, m_max_move_depth});
m_max_move_distance = 99;
for(auto d: m_max_move_distances)
{
m_max_move_distance = std::min(m_max_move_distance, d.second);
}
if (m_max_move_distance <= m_collision_distance &&
std::fabs(m_act_speed) > 0.1 && m_waypoints.size() > 1 ||
m_max_move_distance <= m_collision_distance_near_target &&
......@@ -1006,29 +1038,47 @@ void HomerNavigationNode::calcMaxMoveDist() {
}
}
}
void HomerNavigationNode::ignoreLaserCallback(const std_msgs::String::ConstPtr& msg){
ROS_INFO_STREAM("changed ignore laser to: "<<msg->data);
m_ignore_scan = msg->data;
}
void HomerNavigationNode::maxDepthMoveDistanceCallback(
const std_msgs::Float32::ConstPtr& msg) {
m_max_move_depth = msg->data;
m_max_move_distances["depth"] = msg->data;
calcMaxMoveDist();
}
void HomerNavigationNode::laserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
void HomerNavigationNode::processLaserScan(const sensor_msgs::LaserScan::ConstPtr& msg)
{
m_scan_map[msg->header.frame_id] = msg;
m_last_laser_time = ros::Time::now();
m_max_move_sick = map_tools::get_max_move_distance(
map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link"),
m_min_x, m_min_y);
calcMaxMoveDist();
if(m_MainMachine.state() != IDLE)
{
if(m_ignore_scan.find(msg->header.frame_id)== std::string::npos)
{
m_max_move_distances[msg->header.frame_id] = map_tools::get_max_move_distance(
map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link"),
m_min_x, m_min_y);
}
else
{
m_max_move_distances[msg->header.frame_id] = 99;
}
calcMaxMoveDist();
}
}
void HomerNavigationNode::laserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
processLaserScan(msg);
}
void HomerNavigationNode::downlaserDataCallback(
const sensor_msgs::LaserScan::ConstPtr& msg) {
m_scan_map[msg->header.frame_id] = msg;
m_max_move_down = map_tools::get_max_move_distance(
map_tools::laser_msg_to_points(msg, m_transform_listener, "/base_link"),
m_min_x, m_min_y);
calcMaxMoveDist();
processLaserScan(msg);
}
void HomerNavigationNode::initNewTarget() {
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment