From 40b16f009b1d17765e9ee1fef97c257b580e4ad4 Mon Sep 17 00:00:00 2001 From: pradheep Date: Mon, 20 Jul 2020 16:57:34 +0200 Subject: [PATCH 1/3] added the functionality to estimate the robot's future footprint Software stop enable Now the robot looks at the turns and decides to turn on the collision free path ROS parameter added for enabling the software stop Changed the flag condition for turning Condition added for the narrow path Threshold added to balance the turn Included the footprint rotation Added comments --- include/NeoLocalPlanner.h | 28 ++++++++++++++---- src/NeoLocalPlanner.cpp | 62 +++++++++++++++++++++++++++++++++++++-- symbolics/compile.sh | 0 3 files changed, 82 insertions(+), 8 deletions(-) mode change 100755 => 100644 symbolics/compile.sh diff --git a/include/NeoLocalPlanner.h b/include/NeoLocalPlanner.h index 02312ce..9b9b444 100644 --- a/include/NeoLocalPlanner.h +++ b/include/NeoLocalPlanner.h @@ -41,13 +41,15 @@ #include #include #include +#include #include #include #include - +#include #include #include - +#include +#include namespace neo_local_planner { @@ -64,6 +66,8 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { bool setPlan(const std::vector& plan) override; void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) override; + base_local_planner::WorldModel* Cm; + private: void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); @@ -72,13 +76,14 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { tf2_ros::Buffer* m_tf = 0; costmap_2d::Costmap2DROS* m_cost_map = 0; std::vector m_global_plan; - boost::mutex m_odometry_mutex; nav_msgs::Odometry::ConstPtr m_odometry; + base_local_planner::CostmapModel* model1; ros::Subscriber m_odom_sub; ros::Publisher m_local_plan_pub; + std::string m_global_frame = "map"; std::string m_local_frame = "odom"; std::string m_base_frame = "base_link"; @@ -107,7 +112,11 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { double m_max_cost = 0; // [1] double m_min_stop_dist = 0; // [m] double m_emergency_acc_lim_x = 0; // [m/s^2] + double threshold_cost = 0; + + + bool m_enable_software_stop = true; bool m_differential_drive = false; bool m_constrain_final = false; @@ -129,10 +138,19 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { uint64_t m_update_counter = 0; double m_last_control_values[3] = {}; geometry_msgs::Twist m_last_cmd_vel; - + std::vector footprint_cells; + Eigen::Vector3f Epos; + const costmap_2d::Costmap2D costmap_; + base_local_planner::CostmapModel* world_model_; + double obstacle_in_rot = 0; + double cost_rot_obstacles_left = 1; + double cost_rot_obstacles_right = 1; + bool left_watchout = 0; + bool right_watchout = 0; + }; } // neo_local_planner -#endif /* INCLUDE_NEOLOCALPLANNER_H_ */ +#endif /* INCLUDE_NEOLOCALPLANNER_H_ */ \ No newline at end of file diff --git a/src/NeoLocalPlanner.cpp b/src/NeoLocalPlanner.cpp index a5e481c..d3b1f01 100644 --- a/src/NeoLocalPlanner.cpp +++ b/src/NeoLocalPlanner.cpp @@ -40,7 +40,7 @@ #include #include #include - +#include #include // register this planner as a BaseGlobalPlanner plugin @@ -170,6 +170,7 @@ NeoLocalPlanner::~NeoLocalPlanner() bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { boost::mutex::scoped_lock lock(m_odometry_mutex); + bool obstacles = true; if(!m_odometry) { @@ -227,6 +228,23 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) * tf2::Vector3(start_vel_x, start_vel_y, 0) * m_lookahead_time; actual_yaw = start_yaw + start_yawrate * m_lookahead_time; } + // Determining the presence of obstacles in the footprint + geometry_msgs::Point poses1; + poses1.x = local_pose.getOrigin().x(); + poses1.y = local_pose.getOrigin().y(); + std::vector P1; + P1 = m_cost_map->getRobotFootprint(); + + // Updating the robot footprint + for (int i = 0; igetCostmap()); + obstacle_in_rot = world_model_->footprintCost(poses1, P1, 1.0,1.0); const tf2::Transform actual_pose = tf2::Transform(createQuaternionFromYaw(actual_yaw), actual_pos); // compute cost gradients @@ -462,6 +480,7 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) else { // use term for static target orientation + control_yawrate = yaw_error * m_static_yaw_gain; m_state = state_t::STATE_ROTATING; @@ -551,6 +570,9 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) control_yawrate = control[2]; } } + double temp1 = 0; + temp1 = control_yawrate; + // fill return data cmd_vel.linear.x = fmin(fmax(control_vel_x, m_limits.min_vel_x), m_limits.max_vel_x); @@ -558,7 +580,40 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) cmd_vel.linear.z = 0; cmd_vel.angular.x = 0; cmd_vel.angular.y = 0; - cmd_vel.angular.z = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); + double temp = 0; + temp = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); + + // Footprint based collision avoidance + if(m_enable_software_stop == true) + { + + + if((obstacle_in_rot == -1) && (control_yawrate- start_yawrate < start_yawrate)) + { + ROS_WARN_THROTTLE(1, "During the rotation robot predicted an obstacle on the right! Please free the robot using Joy"); + + cmd_vel.angular.z = 0; + left_watchout == 0; + } + else if((obstacle_in_rot == -1) && (control_yawrate- start_yawrate > start_yawrate)) + { + ROS_WARN_THROTTLE(1, "During the rotation robot predicted an obstacle on the left! Please free the robot using Joy"); + + cmd_vel.angular.z = 0; + right_watchout == 0; + } + else + { + cmd_vel.angular.z = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); + } + + + } + + else + { + cmd_vel.angular.z = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); + } if(m_update_counter % 20 == 0) { ROS_INFO_NAMED("NeoLocalPlanner", "dt=%f, pos_error=(%f, %f), yaw_error=%f, cost=%f, obstacle_dist=%f, obstacle_cost=%f, delta_cost=(%f, %f, %f), state=%d, cmd_vel=(%f, %f), cmd_yawrate=%f", @@ -690,6 +745,7 @@ void NeoLocalPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_ m_max_backup_dist = private_nh.param("max_backup_dist", m_differential_drive ? 0.1 : 0.0); m_min_stop_dist = private_nh.param("min_stop_dist", 0.5); m_emergency_acc_lim_x = private_nh.param("emergency_acc_lim_x", m_limits.acc_lim_x * 4); + m_enable_software_stop = private_nh.param("enable_software_stop", true); m_tf = tf; m_cost_map = costmap_ros; @@ -710,4 +766,4 @@ void NeoLocalPlanner::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) } -} // neo_local_planner +} // neo_local_planner \ No newline at end of file diff --git a/symbolics/compile.sh b/symbolics/compile.sh old mode 100755 new mode 100644 From 6f8a36e360f1a6485119c8620518b23ccf2226f3 Mon Sep 17 00:00:00 2001 From: pradheep Date: Mon, 3 Aug 2020 14:34:04 +0200 Subject: [PATCH 2/3] removed the unwanted lines --- include/NeoLocalPlanner.h | 10 ---------- src/NeoLocalPlanner.cpp | 13 +++---------- 2 files changed, 3 insertions(+), 20 deletions(-) diff --git a/include/NeoLocalPlanner.h b/include/NeoLocalPlanner.h index 9b9b444..cf11554 100644 --- a/include/NeoLocalPlanner.h +++ b/include/NeoLocalPlanner.h @@ -66,7 +66,6 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { bool setPlan(const std::vector& plan) override; void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) override; - base_local_planner::WorldModel* Cm; private: @@ -78,8 +77,6 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { std::vector m_global_plan; boost::mutex m_odometry_mutex; nav_msgs::Odometry::ConstPtr m_odometry; - base_local_planner::CostmapModel* model1; - ros::Subscriber m_odom_sub; ros::Publisher m_local_plan_pub; @@ -112,9 +109,6 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { double m_max_cost = 0; // [1] double m_min_stop_dist = 0; // [m] double m_emergency_acc_lim_x = 0; // [m/s^2] - double threshold_cost = 0; - - bool m_enable_software_stop = true; bool m_differential_drive = false; @@ -141,12 +135,8 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { std::vector footprint_cells; Eigen::Vector3f Epos; const costmap_2d::Costmap2D costmap_; - base_local_planner::CostmapModel* world_model_; double obstacle_in_rot = 0; double cost_rot_obstacles_left = 1; - double cost_rot_obstacles_right = 1; - bool left_watchout = 0; - bool right_watchout = 0; }; diff --git a/src/NeoLocalPlanner.cpp b/src/NeoLocalPlanner.cpp index d3b1f01..ec23ab3 100644 --- a/src/NeoLocalPlanner.cpp +++ b/src/NeoLocalPlanner.cpp @@ -170,7 +170,6 @@ NeoLocalPlanner::~NeoLocalPlanner() bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { boost::mutex::scoped_lock lock(m_odometry_mutex); - bool obstacles = true; if(!m_odometry) { @@ -243,8 +242,8 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) P1[i].y= pos[1]+ actual_pos[1]; } - world_model_ = new base_local_planner::CostmapModel(*m_cost_map->getCostmap()); - obstacle_in_rot = world_model_->footprintCost(poses1, P1, 1.0,1.0); + base_local_planner::CostmapModel world_model_(*m_cost_map->getCostmap()); + const bool obstacle_in_rot = world_model_.footprintCost(poses1, P1, 1.0,1.0); const tf2::Transform actual_pose = tf2::Transform(createQuaternionFromYaw(actual_yaw), actual_pos); // compute cost gradients @@ -570,9 +569,6 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) control_yawrate = control[2]; } } - double temp1 = 0; - temp1 = control_yawrate; - // fill return data cmd_vel.linear.x = fmin(fmax(control_vel_x, m_limits.min_vel_x), m_limits.max_vel_x); @@ -580,8 +576,7 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) cmd_vel.linear.z = 0; cmd_vel.angular.x = 0; cmd_vel.angular.y = 0; - double temp = 0; - temp = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); + cmd_vel.angular.z = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); // Footprint based collision avoidance if(m_enable_software_stop == true) @@ -593,14 +588,12 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) ROS_WARN_THROTTLE(1, "During the rotation robot predicted an obstacle on the right! Please free the robot using Joy"); cmd_vel.angular.z = 0; - left_watchout == 0; } else if((obstacle_in_rot == -1) && (control_yawrate- start_yawrate > start_yawrate)) { ROS_WARN_THROTTLE(1, "During the rotation robot predicted an obstacle on the left! Please free the robot using Joy"); cmd_vel.angular.z = 0; - right_watchout == 0; } else { From 28c4232ccfa0a5b09ab00981d28c514ea96d35df Mon Sep 17 00:00:00 2001 From: pradheep Date: Mon, 3 Aug 2020 14:40:18 +0200 Subject: [PATCH 3/3] Permission changed for Compile sh and some more cleaning up --- include/NeoLocalPlanner.h | 20 ++++++++------------ src/NeoLocalPlanner.cpp | 19 ++----------------- symbolics/compile.sh | 0 3 files changed, 10 insertions(+), 29 deletions(-) mode change 100644 => 100755 symbolics/compile.sh diff --git a/include/NeoLocalPlanner.h b/include/NeoLocalPlanner.h index cf11554..3bf2f0a 100644 --- a/include/NeoLocalPlanner.h +++ b/include/NeoLocalPlanner.h @@ -41,15 +41,16 @@ #include #include #include -#include #include #include #include + #include -#include -#include #include #include +#include +#include + namespace neo_local_planner { @@ -67,7 +68,6 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) override; - private: void odomCallback(const nav_msgs::Odometry::ConstPtr& msg); @@ -75,12 +75,13 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { tf2_ros::Buffer* m_tf = 0; costmap_2d::Costmap2DROS* m_cost_map = 0; std::vector m_global_plan; + boost::mutex m_odometry_mutex; nav_msgs::Odometry::ConstPtr m_odometry; + ros::Subscriber m_odom_sub; ros::Publisher m_local_plan_pub; - std::string m_global_frame = "map"; std::string m_local_frame = "odom"; std::string m_base_frame = "base_link"; @@ -132,15 +133,10 @@ class NeoLocalPlanner : public nav_core::BaseLocalPlanner { uint64_t m_update_counter = 0; double m_last_control_values[3] = {}; geometry_msgs::Twist m_last_cmd_vel; - std::vector footprint_cells; - Eigen::Vector3f Epos; - const costmap_2d::Costmap2D costmap_; - double obstacle_in_rot = 0; - double cost_rot_obstacles_left = 1; - + }; } // neo_local_planner -#endif /* INCLUDE_NEOLOCALPLANNER_H_ */ \ No newline at end of file +#endif /* INCLUDE_NEOLOCALPLANNER_H_ */ diff --git a/src/NeoLocalPlanner.cpp b/src/NeoLocalPlanner.cpp index ec23ab3..5f4330d 100644 --- a/src/NeoLocalPlanner.cpp +++ b/src/NeoLocalPlanner.cpp @@ -40,7 +40,7 @@ #include #include #include -#include + #include // register this planner as a BaseGlobalPlanner plugin @@ -479,7 +479,6 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) else { // use term for static target orientation - control_yawrate = yaw_error * m_static_yaw_gain; m_state = state_t::STATE_ROTATING; @@ -581,8 +580,6 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) // Footprint based collision avoidance if(m_enable_software_stop == true) { - - if((obstacle_in_rot == -1) && (control_yawrate- start_yawrate < start_yawrate)) { ROS_WARN_THROTTLE(1, "During the rotation robot predicted an obstacle on the right! Please free the robot using Joy"); @@ -595,19 +592,7 @@ bool NeoLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) cmd_vel.angular.z = 0; } - else - { - cmd_vel.angular.z = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); - } - - - } - - else - { - cmd_vel.angular.z = fmin(fmax(control_yawrate, -m_limits.max_vel_theta), m_limits.max_vel_theta); } - if(m_update_counter % 20 == 0) { ROS_INFO_NAMED("NeoLocalPlanner", "dt=%f, pos_error=(%f, %f), yaw_error=%f, cost=%f, obstacle_dist=%f, obstacle_cost=%f, delta_cost=(%f, %f, %f), state=%d, cmd_vel=(%f, %f), cmd_yawrate=%f", dt, pos_error.x(), pos_error.y(), yaw_error, center_cost, obstacle_dist, obstacle_cost, delta_cost_x, delta_cost_y, delta_cost_yaw, m_state, control_vel_x, control_vel_y, control_yawrate); @@ -759,4 +744,4 @@ void NeoLocalPlanner::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) } -} // neo_local_planner \ No newline at end of file +} // neo_local_planner diff --git a/symbolics/compile.sh b/symbolics/compile.sh old mode 100644 new mode 100755