changed check path and waypoint angle speed

homer_navigation/config/homer_navigation.yaml

@@ -17,7 +17,7 @@
 
 ### check path on map update
 /homer_navigation/check_path:                     true # toggles if the calculated path will be checked for obstacles while navigating
-/homer_navigation/check_path_max_distance:        2    # m maximal distance from robot position in which the path is being checked for obstacles
+/homer_navigation/check_path_max_distance:        3    # m maximal distance from robot position in which the path is being checked for obstacles
 
 ### speed parameters
 /homer_navigation/min_turn_angle:                 0.1 # rad values lower than this angle will let the navigation assume reaching the designated position   
@@ -29,7 +29,7 @@
 ### caution factors values near 0 mean high caution values greater values mean less caution
 ### if any factor equals 0 the robot can't follow paths !!
 /homer_navigation/map_speed_factor:               0.7 # factor for the max speed calculation of the obstacleDistancemap
-/homer_navigation/waypoint_speed_factor:          10  # factor for the max speed calculation with the distance to the next Waypoint
+/homer_navigation/waypoint_speed_factor:          2  # factor for the max speed calculation with the distance to the next Waypoint
 /homer_navigation/obstacle_speed_factor:          0.5  # factor for the max speed calculation with the last laser max movement distance
 /homer_navigation/target_distance_speed_factor:   0.4  # factor for the max speed calculation with the distance to target
 

homer_navigation/src/homer_navigation_node.cpp

@@ -417,20 +417,33 @@ bool HomerNavigationNode::obstacleOnPath() {
         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");
+                    scan.second, m_transform_listener, "/map");
 
-            for (unsigned i = 0; i < m_pixel_path.size() - 1; i++) {
+            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) {
+                        m_check_path_max_distance * 2) {
                     return false;
                 }
-                for (auto const& sp : scan_points) {
-                    if (map_tools::distance(sp, p) <
-                        m_AllowedObstacleDistance.first) {
-                        ROS_INFO_STREAM("Found obstacle");
-                        return true;
+                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 (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;
+                            }
+                        }
                     }
                 }
             }
@@ -593,12 +606,19 @@ void HomerNavigationNode::performNextMove() {
                                             m_obstacle_speed_factor;
             float max_map_distance_speed =
                 m_max_move_speed * obstacleMapDistance * m_map_speed_factor;
+
+            float max_waypoint_speed = 1;
+            if(m_waypoints.size() > 1)
+            {
+                float angleToNextWaypoint = angleToPointDeg(m_waypoints[1].pose.position);
+                max_waypoint_speed = (1 - (angleToNextWaypoint / 180.0)) * distanceToWaypoint * m_waypoint_speed_factor;
+            }
+
             m_act_speed = std::min(
                 {std::max((float)0.1, m_distance_to_target *
                                           m_target_distance_speed_factor),
                  m_max_move_speed, max_move_distance_speed,
-                 max_map_distance_speed,
-                 distanceToWaypoint * m_waypoint_speed_factor});
+                 max_map_distance_speed, max_waypoint_speed});
             std_msgs::String tmp;
             std::stringstream str;
             str << "m_obstacle_speed " << max_move_distance_speed