diff --git a/include/teb_local_planner/teb_local_planner_ros.h b/include/teb_local_planner/teb_local_planner_ros.h index 78445aef..7f8f80f5 100644 --- a/include/teb_local_planner/teb_local_planner_ros.h +++ b/include/teb_local_planner/teb_local_planner_ros.h @@ -338,6 +338,8 @@ class TebLocalPlannerROS : public nav_core::BaseLocalPlanner */ void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); + void peakViaPoint(double robot_x, double robot_y, double localGoal_x, double localGoal_y, + std::vector& transformed_plan, double robot_radius); //Edit @mudit void configureBackupModes(std::vector& transformed_plan, int& goal_idx); diff --git a/src/teb_local_planner_ros.cpp b/src/teb_local_planner_ros.cpp index 9265cb33..11e9881a 100644 --- a/src/teb_local_planner_ros.cpp +++ b/src/teb_local_planner_ros.cpp @@ -91,21 +91,9 @@ void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, // reserve some memory for obstacles obstacles_.reserve(500); - - // init some variables - tf_ = tf; - costmap_ros_ = costmap_ros; - costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. - - costmap_model_ = boost::make_shared(*costmap_); - - - global_frame_ = costmap_ros_->getGlobalFrameID(); - cfg_.map_frame = global_frame_; // TODO - robot_base_frame_ = costmap_ros_->getBaseFrameID(); // create visualization instance - visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_.map_frame)); + visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); // create robot footprint/contour model for optimization RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh); @@ -122,6 +110,17 @@ void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, ROS_INFO("Parallel planning in distinctive topologies disabled."); } + // init other variables + tf_ = tf; + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. + + costmap_model_ = boost::make_shared(*costmap_); + + global_frame_ = costmap_ros_->getGlobalFrameID(); + cfg_.map_frame = global_frame_; // TODO + robot_base_frame_ = costmap_ros_->getBaseFrameID(); + //Initialize a costmap to polygon converter if (!cfg_.obstacles.costmap_converter_plugin.empty()) { @@ -131,6 +130,7 @@ void TebLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin); // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace boost::replace_all(converter_name, "::", "/"); + costmap_converter_->setOdomTopic(cfg_.odom_topic); costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); costmap_converter_->setCostmap2D(costmap_); @@ -206,6 +206,44 @@ bool TebLocalPlannerROS::setPlan(const std::vector& return true; } +void TebLocalPlannerROS::peakViaPoint(double localGoal_x, double localGoal_y, + std::vector& transformed_plan, double robot_circumscribed_radius) //@mudit +{ + double planLineSlope= (localGoal_y-transformed_plan[1].pose.position.y)/(localGoal_x-transformed_plan[1].pose.position.x); // slope of the line connecting trans. global plan origin and local goal + double planLine_yIntercept= transformed_plan[1].pose.position.y-(planLineSlope*transformed_plan[1].pose.position.x); // y-intercept of the line connecting robot pose and local goal + double max_dist=0; + int viaIdx; + for (std::size_t i=1; i < transformed_plan.size(); ++i) + {double x_transformed= transformed_plan[i].pose.position.x; //x1 + double y_transformed= transformed_plan[i].pose.position.y; //y1 + double c_perpendicular= y_transformed+(1/planLineSlope)*x_transformed; // y-intercept of the line connecting (x1,y1) and the perpendicular point + // meeting the line connecting robot pose and local goal + + double x2= (planLine_yIntercept-c_perpendicular)/((-1/planLineSlope)-planLineSlope); //x2 and y2 are the new points on the line connecting robot pose and local goal + double y2= (planLineSlope*x2)+planLine_yIntercept; + + double sqDist= (x2-x_transformed)*(x2-x_transformed)+(y2-y_transformed)*(y2-y_transformed); //Calculating distance b/w the two points + + if(max_dist(robot_circumscribed_radius/8)) //move_base crashes for unknown reason if max_dist=0.000000 + {via_points_.push_back( Eigen::Vector2d( transformed_plan[viaIdx].pose.position.x, transformed_plan[viaIdx].pose.position.y ) ); + // ROS_INFO("The Distance is %f",max_dist); + //cfg_.hcp.enable_multithreading=false; + // cfg_.hcp.max_number_classes=false; + } + else +{ + //cfg_.hcp.enable_multithreading=true; + // cfg_.hcp.max_number_classes=true; + }// ROS_INFO("Number : %d",cfg_.hcp.max_number_classes); // transformed_plan[i].pose.position.x, transformed_plan[i].pose.position.y +} bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) { @@ -310,6 +348,10 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) // update via-points container updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); + + //@mudit edit : creating a via point for including peak points of the global plan in the robot's path + if(cfg_.trajectory.peak_via_points) + peakViaPoint(robot_goal_.x(),robot_goal_.y(),transformed_plan,robot_circumscribed_radius); // Do not allow config changes during the following optimization step boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); @@ -329,6 +371,13 @@ bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) } // Check feasibility (but within the first few states only) + if(cfg_.robot.is_footprint_dynamic) + { + // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. + footprint_spec_ = costmap_ros_->getRobotFootprint(); + costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); + } + bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses); if (!feasible) { @@ -441,31 +490,37 @@ void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter() return; //Get obstacles from costmap converter - costmap_converter::PolygonContainerConstPtr polygons = costmap_converter_->getPolygons(); - if (!polygons) + costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles(); + if (!obstacles) return; - - for (std::size_t i=0; isize(); ++i) + + for (std::size_t i=0; iobstacles.size(); ++i) { - if (polygons->at(i).points.size()==1) // Point + const geometry_msgs::Polygon* polygon = &obstacles->obstacles.at(i).polygon; + + if (polygon->points.size()==1) // Point { - obstacles_.push_back(ObstaclePtr(new PointObstacle(polygons->at(i).points[0].x, polygons->at(i).points[0].y))); + obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y))); } - else if (polygons->at(i).points.size()==2) // Line + else if (polygon->points.size()==2) // Line { - obstacles_.push_back(ObstaclePtr(new LineObstacle(polygons->at(i).points[0].x, polygons->at(i).points[0].y, - polygons->at(i).points[1].x, polygons->at(i).points[1].y ))); + obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y, + polygon->points[1].x, polygon->points[1].y ))); } - else if (polygons->at(i).points.size()>2) // Real polygon + else if (polygon->points.size()>2) // Real polygon { PolygonObstacle* polyobst = new PolygonObstacle; - for (std::size_t j=0; jat(i).points.size(); ++j) + for (std::size_t j=0; jpoints.size(); ++j) { - polyobst->pushBackVertex(polygons->at(i).points[j].x, polygons->at(i).points[j].y); + polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y); } polyobst->finalizePolygon(); obstacles_.push_back(ObstaclePtr(polyobst)); } + + // Set velocity, if obstacle is moving + if(!obstacles_.empty()) + obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation); } } @@ -474,7 +529,7 @@ void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() { // Add custom obstacles obtained via message boost::mutex::scoped_lock l(custom_obst_mutex_); - + if (!custom_obstacle_msg_.obstacles.empty()) { // We only use the global header to specify the obstacle coordinate system instead of individual ones @@ -496,30 +551,48 @@ void TebLocalPlannerROS::updateObstacleContainerWithCustomObstacles() obstacle_to_map_eig.setIdentity(); } - for (std::vector::const_iterator obst_it = custom_obstacle_msg_.obstacles.begin(); obst_it != custom_obstacle_msg_.obstacles.end(); ++obst_it) + for (size_t i=0; ipolygon.points.size() == 1 ) // point + if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 1 ) // point { - Eigen::Vector3d pos( obst_it->polygon.points.front().x, obst_it->polygon.points.front().y, obst_it->polygon.points.front().z ); + Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x, + custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y, + custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z ); obstacles_.push_back(ObstaclePtr(new PointObstacle( (obstacle_to_map_eig * pos).head(2) ))); } - else if (obst_it->polygon.points.size() == 2 ) // line + else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.size() == 2 ) // line { - Eigen::Vector3d line_start( obst_it->polygon.points.front().x, obst_it->polygon.points.front().y, obst_it->polygon.points.front().z ); - Eigen::Vector3d line_end( obst_it->polygon.points.back().x, obst_it->polygon.points.back().y, obst_it->polygon.points.back().z ); - obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2), (obstacle_to_map_eig * line_end).head(2) ))); + Eigen::Vector3d line_start( custom_obstacle_msg_.obstacles.at(i).polygon.points.front().x, + custom_obstacle_msg_.obstacles.at(i).polygon.points.front().y, + custom_obstacle_msg_.obstacles.at(i).polygon.points.front().z ); + Eigen::Vector3d line_end( custom_obstacle_msg_.obstacles.at(i).polygon.points.back().x, + custom_obstacle_msg_.obstacles.at(i).polygon.points.back().y, + custom_obstacle_msg_.obstacles.at(i).polygon.points.back().z ); + obstacles_.push_back(ObstaclePtr(new LineObstacle( (obstacle_to_map_eig * line_start).head(2), + (obstacle_to_map_eig * line_end).head(2) ))); + } + else if (custom_obstacle_msg_.obstacles.at(i).polygon.points.empty()) + { + ROS_WARN("Invalid custom obstacle received. List of polygon vertices is empty. Skipping..."); + continue; } else // polygon { PolygonObstacle* polyobst = new PolygonObstacle; - for (int i=0; i<(int)obst_it->polygon.points.size(); ++i) + for (size_t j=0; jpolygon.points[i].x, obst_it->polygon.points[i].y, obst_it->polygon.points[i].z ); + Eigen::Vector3d pos( custom_obstacle_msg_.obstacles.at(i).polygon.points[j].x, + custom_obstacle_msg_.obstacles.at(i).polygon.points[j].y, + custom_obstacle_msg_.obstacles.at(i).polygon.points[j].z ); polyobst->pushBackVertex( (obstacle_to_map_eig * pos).head(2) ); } polyobst->finalizePolygon(); obstacles_.push_back(ObstaclePtr(polyobst)); } + + // Set velocity, if obstacle is moving + if(!obstacles_.empty()) + obstacles_.back()->setCentroidVelocity(custom_obstacle_msg_.obstacles[i].velocities, custom_obstacle_msg_.obstacles[i].orientation); } } } @@ -907,7 +980,7 @@ void TebLocalPlannerROS::configureBackupModes(std::vector