From cafcc216fa1f3c1aecc66c3a6acb855152183b55 Mon Sep 17 00:00:00 2001
From: Lisa <robbie@uni-koblenz.de>
Date: Thu, 16 Feb 2017 20:52:38 +0100
Subject: [PATCH] map_manager without laser scans
---
homer_map_manager/src/Managers/MapManager.cpp | 244 +++++++++---------
homer_nav_libs/src/Explorer/Explorer.cpp | 101 ++++++--
.../homer_navigation/homer_navigation_node.h | 13 +-
.../src/homer_navigation_node.cpp | 182 ++++++++-----
4 files changed, 322 insertions(+), 218 deletions(-)
diff --git a/homer_map_manager/src/Managers/MapManager.cpp b/homer_map_manager/src/Managers/MapManager.cpp
index bbf2301d..40e1cc45 100644
--- a/homer_map_manager/src/Managers/MapManager.cpp
+++ b/homer_map_manager/src/Managers/MapManager.cpp
@@ -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";
diff --git a/homer_nav_libs/src/Explorer/Explorer.cpp b/homer_nav_libs/src/Explorer/Explorer.cpp
index 2eaec66c..a209d748 100644
--- a/homer_nav_libs/src/Explorer/Explorer.cpp
+++ b/homer_nav_libs/src/Explorer/Explorer.cpp
@@ -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
diff --git a/homer_navigation/include/homer_navigation/homer_navigation_node.h b/homer_navigation/include/homer_navigation/homer_navigation_node.h
index d452feb4..21ad99f9 100644
--- a/homer_navigation/include/homer_navigation/homer_navigation_node.h
+++ b/homer_navigation/include/homer_navigation/homer_navigation_node.h
@@ -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;
diff --git a/homer_navigation/src/homer_navigation_node.cpp b/homer_navigation/src/homer_navigation_node.cpp
index 69a7c888..2b0296eb 100644
--- a/homer_navigation/src/homer_navigation_node.cpp
+++ b/homer_navigation/src/homer_navigation_node.cpp
@@ -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() {
--
GitLab