diff --git a/mpc_local_planner_ros.cpp b/mpc_local_planner_ros.cpp new file mode 100644 index 0000000..c7eb0f5 --- /dev/null +++ b/mpc_local_planner_ros.cpp @@ -0,0 +1,1150 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#include + +#include + +#include + +#include +#include + +#include + +// MBF return codes +#include + +// pluginlib macros +#include + +// register this planner both as a BaseLocalPlanner and as a MBF's CostmapController plugin +PLUGINLIB_EXPORT_CLASS(mpc_local_planner::MpcLocalPlannerROS, nav_core::BaseLocalPlanner) +PLUGINLIB_EXPORT_CLASS(mpc_local_planner::MpcLocalPlannerROS, mbf_costmap_core::CostmapController) + +namespace mpc_local_planner { + +MpcLocalPlannerROS::MpcLocalPlannerROS() + : _costmap_ros(nullptr), + _tf(nullptr), + _costmap_model(nullptr), + _costmap_converter_loader("costmap_converter", "costmap_converter::BaseCostmapToPolygons"), + /*dynamic_recfg_(NULL),*/ + _goal_reached(false), + _no_infeasible_plans(0), + /*last_preferred_rotdir_(RotType::none),*/ + _initialized(false) +{ +} + +MpcLocalPlannerROS::~MpcLocalPlannerROS() {} + +/* +void MpcLocalPlannerROS::reconfigureCB(TebLocalPlannerReconfigureConfig& config, uint32_t level) +{ + cfg_.reconfigure(config); +} +*/ + +//void MpcLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) +void MpcLocalPlannerROS::initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) +{ + // check if the plugin is already initialized + if (!_initialized) + { + // create Node Handle with name of plugin (as used in move_base for loading) + ros::NodeHandle nh("~/" + name); + + // load plugin related main parameters + nh.param("controller/xy_goal_tolerance", _params.xy_goal_tolerance, _params.xy_goal_tolerance); + nh.param("controller/yaw_goal_tolerance", _params.yaw_goal_tolerance, _params.yaw_goal_tolerance); + nh.param("controller/global_plan_overwrite_orientation", _params.global_plan_overwrite_orientation, + _params.global_plan_overwrite_orientation); + nh.param("controller/global_plan_prune_distance", _params.global_plan_prune_distance, _params.global_plan_prune_distance); + nh.param("controller/max_global_plan_lookahead_dist", _params.max_global_plan_lookahead_dist, _params.max_global_plan_lookahead_dist); + nh.param("controller/global_plan_viapoint_sep", _params.global_plan_viapoint_sep, _params.global_plan_viapoint_sep); + _controller.setInitialPlanEstimateOrientation(_params.global_plan_overwrite_orientation); + + // special parameters + nh.param("odom_topic", _params.odom_topic, _params.odom_topic); + nh.param("footprint_model/is_footprint_dynamic", _params.is_footprint_dynamic, _params.is_footprint_dynamic); + nh.param("collision_avoidance/include_costmap_obstacles", _params.include_costmap_obstacles, _params.include_costmap_obstacles); + nh.param("collision_avoidance/costmap_obstacles_behind_robot_dist", _params.costmap_obstacles_behind_robot_dist, + _params.costmap_obstacles_behind_robot_dist); + + nh.param("collision_avoidance/collision_check_no_poses", _params.collision_check_no_poses, _params.collision_check_no_poses); + nh.param("collision_avoidance/collision_check_min_resolution_angular", _params.collision_check_min_resolution_angular, + _params.collision_check_min_resolution_angular); + + // costmap converter plugin related parameters + nh.param("costmap_converter_plugin", _costmap_conv_params.costmap_converter_plugin, _costmap_conv_params.costmap_converter_plugin); + nh.param("costmap_converter_rate", _costmap_conv_params.costmap_converter_rate, _costmap_conv_params.costmap_converter_rate); + nh.param("costmap_converter_spin_thread", _costmap_conv_params.costmap_converter_spin_thread, + _costmap_conv_params.costmap_converter_spin_thread); + + // reserve some memory for obstacles + _obstacles.reserve(700); + + // init other variables + _tf = tf; + _costmap_ros = costmap_ros; + _costmap = _costmap_ros->getCostmap(); // locking should be done in MoveBase. + + _costmap_model = std::make_shared(*_costmap); + + _global_frame = _costmap_ros->getGlobalFrameID(); + _robot_base_frame = _costmap_ros->getBaseFrameID(); + + // create robot footprint/contour model for optimization + _robot_model = getRobotFootprintFromParamServer(nh, _costmap_ros); + + // create the planner instance + if (!_controller.configure(nh, _obstacles, _robot_model, _via_points)) + { + ROS_ERROR("Controller configuration failed."); + return; + } + + // create visualization instance + _publisher.initialize(nh, _controller.getRobotDynamics(), _global_frame); + + // Initialize a costmap to polygon converter + if (!_costmap_conv_params.costmap_converter_plugin.empty()) + { + try + { + _costmap_converter = _costmap_converter_loader.createInstance(_costmap_conv_params.costmap_converter_plugin); + std::string converter_name = _costmap_converter_loader.getName(_costmap_conv_params.costmap_converter_plugin); + // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace + boost::replace_all(converter_name, "::", "/"); + _costmap_converter->setOdomTopic(_params.odom_topic); + _costmap_converter->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); + _costmap_converter->setCostmap2D(_costmap); + + _costmap_converter->startWorker(ros::Rate(_costmap_conv_params.costmap_converter_rate), _costmap, + _costmap_conv_params.costmap_converter_spin_thread); + ROS_INFO_STREAM("Costmap conversion plugin " << _costmap_conv_params.costmap_converter_plugin << " loaded."); + } + catch (pluginlib::PluginlibException& ex) + { + ROS_WARN( + "The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error " + "message: %s", + ex.what()); + _costmap_converter.reset(); + } + } + else + ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); + + // Get 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); + + // init the odom helper to receive the robot's velocity from odom messages + _odom_helper.setOdomTopic(_params.odom_topic); + + // setup dynamic reconfigure + /* + dynamic_recfg_ = boost::make_shared>(nh); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&MpcLocalPlanner::reconfigureCB, this, _1, _2); + dynamic_recfg_->setCallback(cb); + */ + + // validate optimization footprint and costmap footprint + validateFootprints(_robot_model->getInscribedRadius(), _robot_inscribed_radius, _controller.getInequalityConstraint()->getMinimumDistance()); + + // setup callback for custom obstacles + _custom_obst_sub = nh.subscribe("obstacles", 1, &MpcLocalPlannerROS::customObstacleCB, this); + + // setup callback for custom via-points + _via_points_sub = nh.subscribe("via_points", 1, &MpcLocalPlannerROS::customViaPointsCB, this); + + // additional move base params + ros::NodeHandle nh_move_base("~"); + nh_move_base.param("controller_frequency", _params.controller_frequency, _params.controller_frequency); + + // set initialized flag + _initialized = true; + + ROS_DEBUG("mpc_local_planner plugin initialized."); + } + else + { + ROS_WARN("mpc_local_planner has already been initialized, doing nothing."); + } +} + +bool MpcLocalPlannerROS::setPlan(const std::vector& orig_global_plan) +{ + // check if plugin is initialized + if (!_initialized) + { + ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); + return false; + } + + // store the global plan + _global_plan.clear(); + _global_plan = orig_global_plan; + + // we do not clear the local planner here, since setPlan is called frequently whenever the global planner updates the plan. + // the local planner checks whether it is required to reinitialize the trajectory or not within each velocity computation step. + + // reset goal_reached_ flag + _goal_reached = false; + + return true; +} + +bool MpcLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) +{ + std::string dummy_message; + geometry_msgs::PoseStamped dummy_pose; + geometry_msgs::TwistStamped dummy_velocity, cmd_vel_stamped; + uint32_t outcome = computeVelocityCommands(dummy_pose, dummy_velocity, cmd_vel_stamped, dummy_message); + cmd_vel = cmd_vel_stamped.twist; + return outcome == mbf_msgs::ExePathResult::SUCCESS; +} + +uint32_t MpcLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, + geometry_msgs::TwistStamped& cmd_vel, std::string& message) +{ + // check if plugin initialized + if (!_initialized) + { + ROS_ERROR("mpc_local_planner has not been initialized, please call initialize() before using this planner"); + message = "mpc_local_planner has not been initialized"; + return mbf_msgs::ExePathResult::NOT_INITIALIZED; + } + + static uint32_t seq = 0; + cmd_vel.header.seq = seq++; + cmd_vel.header.stamp = ros::Time::now(); + cmd_vel.header.frame_id = _robot_base_frame; + cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; + _goal_reached = false; + + // Get robot pose + geometry_msgs::PoseStamped robot_pose; + + tf::Stamped robot_pose_; + _costmap_ros->getRobotPose(robot_pose_); + + robot_pose.header.stamp = robot_pose_.stamp_; + robot_pose.header.frame_id = robot_pose_.frame_id_; + robot_pose.pose.position.x = robot_pose_.getOrigin().getX(); + robot_pose.pose.position.y = robot_pose_.getOrigin().getY(); + robot_pose.pose.position.z = 0; + robot_pose.pose.orientation.x = robot_pose_.getRotation().x(); + robot_pose.pose.orientation.y = robot_pose_.getRotation().y(); + robot_pose.pose.orientation.z = robot_pose_.getRotation().z(); + robot_pose.pose.orientation.w = robot_pose_.getRotation().w(); + + _robot_pose = PoseSE2(robot_pose.pose); + + // Get robot velocity + //geometry_msgs::PoseStamped robot_vel_tf; + tf::Stamped robot_vel_tf; + _odom_helper.getRobotVel(robot_vel_tf); + //_robot_vel.linear.x = robot_vel_tf.pose.position.x; + //_robot_vel.linear.y = robot_vel_tf.pose.position.y; + //_robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); + + _robot_vel.linear.x = robot_vel_tf.getOrigin().getX(); + _robot_vel.linear.y = robot_vel_tf.getOrigin().getY(); + _robot_vel.angular.z = tf::getYaw(robot_vel_tf.getRotation()); + + // prune global plan to cut off parts of the past (spatially before the robot) + pruneGlobalPlan(*_tf, robot_pose_, _global_plan, _params.global_plan_prune_distance); + + // Transform global plan to the frame of interest (w.r.t. the local costmap) + std::vector transformed_plan; + int goal_idx; + //geometry_msgs::TransformStamped tf_plan_to_global; + tf::StampedTransform tf_plan_to_global; + if (!transformGlobalPlan(*_tf, _global_plan, robot_pose_, *_costmap, _global_frame, _params.max_global_plan_lookahead_dist, transformed_plan, + &goal_idx, &tf_plan_to_global)) + { + ROS_WARN("Could not transform the global plan to the frame of the controller"); + message = "Could not transform the global plan to the frame of the controller"; + return mbf_msgs::ExePathResult::INTERNAL_ERROR; + } + + // update via-points container + if (!_custom_via_points_active) + updateViaPointsContainer(transformed_plan, _params.global_plan_viapoint_sep); + + // check if global goal is reached + //geometry_msgs::PoseStamped global_goal; + tf::Stamped global_goal; + //tf2::doTransform(_global_plan.back(), global_goal, tf_plan_to_global); + tf::poseStampedMsgToTF(_global_plan.back(), global_goal); + global_goal.setData( tf_plan_to_global * global_goal ); + //double dx = global_goal.pose.position.x - _robot_pose.x(); + //double dy = global_goal.pose.position.y - _robot_pose.y(); + //double delta_orient = g2o::normalize_theta(tf2::getYaw(global_goal.pose.orientation) - _robot_pose.theta()); + double dx = global_goal.getOrigin().getX() - _robot_pose.x(); + double dy = global_goal.getOrigin().getY() - _robot_pose.y(); + //double delta_orient = g2o::normalize_theta(tf2::getYaw(global_goal.pose.orientation) - _robot_pose.theta()); + double delta_orient = g2o::normalize_theta( tf::getYaw(global_goal.getRotation()) - _robot_pose.theta() ); + if (std::abs(std::sqrt(dx * dx + dy * dy)) < _params.xy_goal_tolerance + && std::abs(delta_orient) < _params.yaw_goal_tolerance) + { + _goal_reached = true; + return mbf_msgs::ExePathResult::SUCCESS; + } + + // Return false if the transformed global plan is empty + if (transformed_plan.empty()) + { + ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); + message = "Transformed plan is empty"; + return mbf_msgs::ExePathResult::INVALID_PATH; + } + + // Get current goal point (last point of the transformed plan) + tf::Stamped goal_point; + tf::poseStampedMsgToTF(transformed_plan.back(), goal_point); + //_robot_goal.x() = transformed_plan.back().pose.position.x; + //_robot_goal.y() = transformed_plan.back().pose.position.y; + _robot_goal.x() = goal_point.getOrigin().getX(); + _robot_goal.y() = goal_point.getOrigin().getY(); + // Overwrite goal orientation if needed + if (_params.global_plan_overwrite_orientation) + { + //_robot_goal.theta() = estimateLocalGoalOrientation(_global_plan, transformed_plan.back(), goal_idx, tf_plan_to_global); + _robot_goal.theta() = estimateLocalGoalOrientation(_global_plan, goal_point, goal_idx, tf_plan_to_global); + // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) + //tf2::Quaternion q; + //q.setRPY(0, 0, _robot_goal.theta()); + //tf2::convert(q, transformed_plan.back().pose.orientation); + } + else + { + //_robot_goal.theta() = tf2::getYaw(transformed_plan.back().pose.orientation); + _robot_goal.theta() = tf::getYaw(goal_point.getRotation()); + } + + // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) + if (transformed_plan.size() == 1) // plan only contains the goal + { + transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) + } + transformed_plan.front() = robot_pose; // update start + + // clear currently existing obstacles + _obstacles.clear(); + + // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin + if (_costmap_converter) + updateObstacleContainerWithCostmapConverter(); + else + updateObstacleContainerWithCostmap(); + + // also consider custom obstacles (must be called after other updates, since the container is not cleared) + updateObstacleContainerWithCustomObstacles(); + + // estimate current state vector and previous control + // updateControlFromTwist() + + // Do not allow config changes during the following optimization step + // TODO(roesmann): we need something similar in case we allow dynamic reconfiguration: + // boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); + + // Now perform the actual planning + // bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); + ros::Time t = ros::Time::now(); + // controller_frequency might not be established in case planning takes longer: + // value dt affects e.g. control deviation bounds (acceleration limits) and we want a goot start value + // let's test how it works with the expected frequency instead of using the actual one + double dt = 1.0 / _params.controller_frequency; + // double dt = time_last_cmd_.isZero() ? 0.0 : (t - time_last_cmd_).toSec(); + + // set previous control value for control deviation bounds + if (_u_seq && !_u_seq->isEmpty()) _controller.getOptimalControlProblem()->setPreviousControlInput(_u_seq->getValuesMap(0), dt); + + bool success = false; + + { + std::lock_guard vp_lock(_via_point_mutex); + std::lock_guard obst_lock(_custom_obst_mutex); + success = _controller.step(transformed_plan, _robot_vel, dt, t, _u_seq, _x_seq); + } + + if (!success) + { + _controller.reset(); // force reinitialization for next time + ROS_WARN("mpc_local_planner was not able to obtain a local plan for the current setting."); + + ++_no_infeasible_plans; // increase number of infeasible solutions in a row + _time_last_infeasible_plan = ros::Time::now(); + _last_cmd = cmd_vel.twist; + message = "mpc_local_planner was not able to obtain a local plan"; + return mbf_msgs::ExePathResult::NO_VALID_CMD; + } + + // Check feasibility (but within the first few states only) + if (_params.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 = _controller.isPoseTrajectoryFeasible(_costmap_model.get(), _footprint_spec, _robot_inscribed_radius, _robot_circumscribed_radius, + _params.collision_check_min_resolution_angular, _params.collision_check_no_poses); + if (!feasible) + { + cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; + + // now we reset everything to start again with the initialization of new trajectories. + _controller.reset(); // force reinitialization for next time + ROS_WARN("MpcLocalPlannerROS: trajectory is not feasible. Resetting planner..."); + ++_no_infeasible_plans; // increase number of infeasible solutions in a row + _time_last_infeasible_plan = ros::Time::now(); + _last_cmd = cmd_vel.twist; + message = "mpc_local_planner trajectory is not feasible"; + return mbf_msgs::ExePathResult::NO_VALID_CMD; + } + + // Get the velocity command for this sampling interval + // TODO(roesmann): we might also command more than just the imminent action, e.g. in a separate thread, until a new command is available + if (!_u_seq || !_controller.getRobotDynamics()->getTwistFromControl(_u_seq->getValuesMap(0), cmd_vel.twist)) + { + _controller.reset(); + ROS_WARN("MpcLocalPlannerROS: velocity command invalid. Resetting controller..."); + ++_no_infeasible_plans; // increase number of infeasible solutions in a row + _time_last_infeasible_plan = ros::Time::now(); + _last_cmd = cmd_vel.twist; + message = "mpc_local_planner velocity command invalid"; + return mbf_msgs::ExePathResult::NO_VALID_CMD; + } + + // Saturate velocity, if the optimization results violates the constraints (could be possible due to early termination or soft cosntraints). + // saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, + // cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); + + // a feasible solution should be found, reset counter + _no_infeasible_plans = 0; + + // store last command (for recovery analysis etc.) + _last_cmd = cmd_vel.twist; + _time_last_cmd = ros::Time::now(); + + // Now visualize everything + _publisher.publishLocalPlan(*_x_seq); + _publisher.publishObstacles(_obstacles); + _publisher.publishGlobalPlan(_global_plan); + _publisher.publishViaPoints(_via_points); + _publisher.publishRobotFootprintModel(_robot_pose, *_robot_model); + return mbf_msgs::ExePathResult::SUCCESS; +} + +bool MpcLocalPlannerROS::isGoalReached() +{ + if (_goal_reached) + { + ROS_INFO("GOAL Reached!"); + // planner_->clearPlanner(); + return true; + } + return false; +} + +void MpcLocalPlannerROS::updateObstacleContainerWithCostmap() +{ + // Add costmap obstacles if desired + if (_params.include_costmap_obstacles) + { + Eigen::Vector2d robot_orient = _robot_pose.orientationUnitVec(); + + for (unsigned int i = 0; i < _costmap->getSizeInCellsX() - 1; ++i) + { + for (unsigned int j = 0; j < _costmap->getSizeInCellsY() - 1; ++j) + { + if (_costmap->getCost(i, j) == costmap_2d::LETHAL_OBSTACLE) + { + Eigen::Vector2d obs; + _costmap->mapToWorld(i, j, obs.coeffRef(0), obs.coeffRef(1)); + + // check if obstacle is interesting (e.g. not far behind the robot) + Eigen::Vector2d obs_dir = obs - _robot_pose.position(); + if (obs_dir.dot(robot_orient) < 0 && obs_dir.norm() > _params.costmap_obstacles_behind_robot_dist) continue; + + _obstacles.push_back(ObstaclePtr(new PointObstacle(obs))); + } + } + } + } +} + +void MpcLocalPlannerROS::updateObstacleContainerWithCostmapConverter() +{ + if (!_costmap_converter) return; + + // Get obstacles from costmap converter + costmap_converter::ObstacleArrayConstPtr obstacles = _costmap_converter->getObstacles(); + if (!obstacles) return; + + for (std::size_t i = 0; i < obstacles->obstacles.size(); ++i) + { + const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i); + const geometry_msgs::Polygon* polygon = &obstacle->polygon; + + if (polygon->points.size() == 1 && obstacle->radius > 0) // Circle + { + _obstacles.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius))); + } + else if (polygon->points.size() == 1) // Point + { + _obstacles.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y))); + } + else if (polygon->points.size() == 2) // Line + { + _obstacles.push_back( + ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y, polygon->points[1].x, polygon->points[1].y))); + } + else if (polygon->points.size() > 2) // Real polygon + { + PolygonObstacle* polyobst = new PolygonObstacle; + for (std::size_t j = 0; j < polygon->points.size(); ++j) + { + 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); + } +} + +void MpcLocalPlannerROS::updateObstacleContainerWithCustomObstacles() +{ + // Add custom obstacles obtained via message + std::lock_guard 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 + Eigen::Affine3d obstacle_to_map_eig; + try + { + tf::StampedTransform obstacle_to_map; + _tf->waitForTransform(_global_frame, ros::Time(0), + _custom_obstacle_msg.header.frame_id, ros::Time(0), + _custom_obstacle_msg.header.frame_id, ros::Duration(0.5)); + _tf->lookupTransform(_global_frame, ros::Time(0), + _custom_obstacle_msg.header.frame_id, ros::Time(0), + _custom_obstacle_msg.header.frame_id, obstacle_to_map); + //obstacle_to_map_eig = tf2::transformToEigen(obstacle_to_map); + tf::transformTFToEigen(obstacle_to_map, obstacle_to_map_eig); + } + catch (tf::TransformException ex) + { + ROS_ERROR("%s", ex.what()); + obstacle_to_map_eig.setIdentity(); + } + + for (size_t i = 0; i < _custom_obstacle_msg.obstacles.size(); ++i) + { + if (_custom_obstacle_msg.obstacles.at(i).polygon.points.size() == 1 && _custom_obstacle_msg.obstacles.at(i).radius > 0) // circle + { + 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 CircularObstacle((obstacle_to_map_eig * pos).head(2), _custom_obstacle_msg.obstacles.at(i).radius))); + } + else if (_custom_obstacle_msg.obstacles.at(i).polygon.points.size() == 1) // point + { + 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 (_custom_obstacle_msg.obstacles.at(i).polygon.points.size() == 2) // line + { + 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 (size_t j = 0; j < _custom_obstacle_msg.obstacles.at(i).polygon.points.size(); ++j) + { + 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); + } + } +} + +void MpcLocalPlannerROS::updateViaPointsContainer(const std::vector& transformed_plan, double min_separation) +{ + _via_points.clear(); + + if (min_separation <= 0) return; + + std::size_t prev_idx = 0; + for (std::size_t i = 1; i < transformed_plan.size(); ++i) // skip first one, since we do not need any point before the first min_separation [m] + { + // check separation to the previous via-point inserted + if (distance_points2d(transformed_plan[prev_idx].pose.position, transformed_plan[i].pose.position) < min_separation) continue; + + // add via-point + _via_points.emplace_back(transformed_plan[i].pose); + prev_idx = i; + } +} + +Eigen::Vector2d MpcLocalPlannerROS::tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel) +{ + Eigen::Vector2d vel; + vel.coeffRef(0) = std::sqrt(tf_vel.getOrigin().getX() * tf_vel.getOrigin().getX() + tf_vel.getOrigin().getY() * tf_vel.getOrigin().getY()); + vel.coeffRef(1) = tf::getYaw(tf_vel.getRotation()); + return vel; +} + +//bool MpcLocalPlannerROS::pruneGlobalPlan(const tf2_ros::Buffer& tf, const geometry_msgs::PoseStamped& global_pose, +bool MpcLocalPlannerROS::pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped& global_pose, + std::vector& global_plan, double dist_behind_robot) +{ + if (global_plan.empty()) + return true; + + try + { + // transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times) + //geometry_msgs::TransformStamped global_to_plan_transform; + tf::StampedTransform global_to_plan_transform; + tf.lookupTransform(global_plan.front().header.frame_id, global_pose.frame_id_, ros::Time(0),global_to_plan_transform); + //geometry_msgs::PoseStamped robot; + //tf2::doTransform(global_pose, robot, global_to_plan_transform); + tf::Stamped robot; + robot.setData( global_to_plan_transform * global_pose ); + + double dist_thresh_sq = dist_behind_robot * dist_behind_robot; + + // iterate plan until a pose close the robot is found + std::vector::iterator it = global_plan.begin(); + std::vector::iterator erase_end = it; + while (it != global_plan.end()) + { + //double dx = robot.pose.position.x - it->pose.position.x; + //double dy = robot.pose.position.y - it->pose.position.y; + //double dist_sq = dx * dx + dy * dy; + double dx = robot.getOrigin().x() - it->pose.position.x; + double dy = robot.getOrigin().y() - it->pose.position.y; + double dist_sq = dx * dx + dy * dy; + if (dist_sq < dist_thresh_sq) + { + erase_end = it; + break; + } + ++it; + } + if (erase_end == global_plan.end()) + return false; + + if (erase_end != global_plan.begin()) + global_plan.erase(global_plan.begin(), erase_end); + } + catch (const tf::TransformException& ex) + { + ROS_DEBUG("Cannot prune path since no transform is available: %s\n", ex.what()); + return false; + } + return true; +} + +//bool MpcLocalPlannerROS::transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector& global_plan, +bool MpcLocalPlannerROS::transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, + const tf::Stamped& global_pose, const costmap_2d::Costmap2D& costmap, + const std::string& global_frame, double max_plan_length, + std::vector& transformed_plan, int* current_goal_idx, + tf::StampedTransform* tf_plan_to_global) const +{ + // this method is a slightly modified version of base_local_planner/goal_functions.h + + const geometry_msgs::PoseStamped& plan_pose = global_plan[0]; + + transformed_plan.clear(); + + try + { + if (global_plan.empty()) + { + ROS_ERROR("Received plan with zero length"); + *current_goal_idx = 0; + return false; + } + + // get plan_to_global_transform from plan frame to global_frame + //geometry_msgs::TransformStamped plan_to_global_transform ; + tf::StampedTransform plan_to_global_transform; + tf.waitForTransform(global_frame, ros::Time::now(),plan_pose.header.frame_id, plan_pose.header.stamp, + plan_pose.header.frame_id, ros::Duration(0.5)); + tf.lookupTransform(global_frame, ros::Time(), + plan_pose.header.frame_id, plan_pose.header.stamp, + plan_pose.header.frame_id, plan_to_global_transform); + + // let's get the pose of the robot in the frame of the plan + //geometry_msgs::PoseStamped robot_pose; + tf::Stamped robot_pose; + //tf.transform(global_pose, robot_pose, plan_pose.header.frame_id); + tf.transformPose(plan_pose.header.frame_id, global_pose, robot_pose); + + + // we'll discard points on the plan that are outside the local costmap + double dist_threshold = + std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0, costmap.getSizeInCellsY() * costmap.getResolution() / 2.0); + dist_threshold *= 0.85; // just consider 85% of the costmap size to better incorporate point obstacle that are + // located on the border of the local costmap + + int i = 0; + double sq_dist_threshold = dist_threshold * dist_threshold; + double sq_dist = 1e10; + + // we need to loop to a point on the plan that is within a certain distance of the robot + for (int j = 0; j < (int)global_plan.size(); ++j) + { + //double x_diff = robot_pose.pose.position.x - global_plan[j].pose.position.x; + //double y_diff = robot_pose.pose.position.y - global_plan[j].pose.position.y; + double x_diff = robot_pose.getOrigin().x() - global_plan[j].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan[j].pose.position.y; + double new_sq_dist = x_diff * x_diff + y_diff * y_diff; + if (new_sq_dist > sq_dist_threshold) + break; // force stop if we have reached the costmap border + + if (new_sq_dist < sq_dist) // find closest distance + { + sq_dist = new_sq_dist; + i = j; + } + } + + tf::Stamped tf_pose; + geometry_msgs::PoseStamped newer_pose; + + double plan_length = 0; // check cumulative Euclidean distance along the plan + + // now we'll transform until points are outside of our distance threshold + while (i < (int)global_plan.size() && sq_dist <= sq_dist_threshold && (max_plan_length <= 0 || plan_length <= max_plan_length)) + { + const geometry_msgs::PoseStamped& pose = global_plan[i]; + //tf2::doTransform(pose, newer_pose, plan_to_global_transform); + tf::poseStampedMsgToTF(pose, tf_pose); + tf_pose.setData(plan_to_global_transform * tf_pose); + tf_pose.stamp_ = plan_to_global_transform.stamp_; + tf_pose.frame_id_ = global_frame; + tf::poseStampedTFToMsg(tf_pose, newer_pose); + + transformed_plan.push_back(newer_pose); + + //double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x; + //double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y; + double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x; + double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y; + sq_dist = x_diff * x_diff + y_diff * y_diff; + + // caclulate distance to previous pose + if (i > 0 && max_plan_length > 0) + plan_length += teb_local_planner::distance_points2d(global_plan[i - 1].pose.position, global_plan[i].pose.position); + + ++i; + } + + // if we are really close to the goal (>0) + // the resulting transformed plan can be empty. In that case we explicitly inject the global goal. + if (transformed_plan.empty()) + { + //tf2::doTransform(global_plan.back(), newer_pose, plan_to_global_transform); + tf::poseStampedMsgToTF(global_plan.back(), tf_pose); + tf_pose.setData(plan_to_global_transform * tf_pose); + tf_pose.stamp_ = plan_to_global_transform.stamp_; + tf_pose.frame_id_ = global_frame; + tf::poseStampedTFToMsg(tf_pose, newer_pose); + + transformed_plan.push_back(newer_pose); + + // Return the index of the current goal point (inside the distance threshold) + if (current_goal_idx) *current_goal_idx = int(global_plan.size()) - 1; + } + else + { + // Return the index of the current goal point (inside the distance threshold) + if (current_goal_idx) *current_goal_idx = i - 1; // subtract 1, since i was increased once before leaving the loop + } + + // Return the transformation from the global plan to the global planning frame if desired + if (tf_plan_to_global) *tf_plan_to_global = plan_to_global_transform; + } + catch (tf::LookupException& ex) + { + ROS_ERROR("No Transform available Error: %s\n", ex.what()); + return false; + } + catch (tf::ConnectivityException& ex) + { + ROS_ERROR("Connectivity Error: %s\n", ex.what()); + return false; + } + catch (tf::ExtrapolationException& ex) + { + ROS_ERROR("Extrapolation Error: %s\n", ex.what()); + if (global_plan.size() > 0) + ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), + global_plan[0].header.frame_id.c_str()); + + return false; + } + + return true; +} + +double MpcLocalPlannerROS::estimateLocalGoalOrientation(const std::vector& global_plan, + const tf::Stamped& local_goal, int current_goal_idx, + const tf::StampedTransform& tf_plan_to_global, int moving_average_length) const +{ + int n = (int)global_plan.size(); + + // check if we are near the global goal already + if (current_goal_idx > n - moving_average_length - 2) + { + if (current_goal_idx >= n - 1) // we've exactly reached the goal + { + //return tf2::getYaw(local_goal.pose.orientation); + return tf::getYaw(local_goal.getRotation()); + } + else + { + //tf2::Quaternion global_orientation; + //tf2::convert(global_plan.back().pose.orientation, global_orientation); + //tf2::Quaternion rotation; + //tf2::convert(tf_plan_to_global.transform.rotation, rotation); + // TODO(roesmann): avoid conversion to tf2::Quaternion + //return tf2::getYaw(rotation * global_orientation); + tf::Quaternion global_orientation; + tf::quaternionMsgToTF(global_plan.back().pose.orientation, global_orientation); + return tf::getYaw(tf_plan_to_global.getRotation() * global_orientation ); + } + } + + // reduce number of poses taken into account if the desired number of poses is not available + moving_average_length = + std::min(moving_average_length, n - current_goal_idx - 1); // maybe redundant, since we have checked the vicinity of the goal before + + std::vector candidates; + //geometry_msgs::PoseStamped tf_pose_k = local_goal; + //geometry_msgs::PoseStamped tf_pose_kp1; + tf::Stamped tf_pose_k = local_goal; + tf::Stamped tf_pose_kp1; + + int range_end = current_goal_idx + moving_average_length; + for (int i = current_goal_idx; i < range_end; ++i) + { + // Transform pose of the global plan to the planning frame + //tf2::doTransform(global_plan.at(i + 1), tf_pose_kp1, tf_plan_to_global); + const geometry_msgs::PoseStamped& pose = global_plan.at(i+1); + tf::poseStampedMsgToTF(pose, tf_pose_kp1); + tf_pose_kp1.setData(tf_plan_to_global * tf_pose_kp1); + + // calculate yaw angle + //candidates.push_back( + // std::atan2(tf_pose_kp1.pose.position.y - tf_pose_k.pose.position.y, tf_pose_kp1.pose.position.x - tf_pose_k.pose.position.x)); + candidates.push_back( std::atan2(tf_pose_kp1.getOrigin().getY() - tf_pose_k.getOrigin().getY(), + tf_pose_kp1.getOrigin().getX() - tf_pose_k.getOrigin().getX() ) ); + + if (i < range_end - 1) + tf_pose_k = tf_pose_kp1; + } + return teb_local_planner::average_angles(candidates); +} + +void MpcLocalPlannerROS::validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist) +{ + ROS_WARN_COND(opt_inscribed_radius + min_obst_dist < costmap_inscribed_radius, + "The inscribed radius of the footprint specified for TEB optimization (%f) + min_obstacle_dist (%f) are smaller " + "than the inscribed radius of the robot's footprint in the costmap parameters (%f, including 'footprint_padding'). " + "Infeasible optimziation results might occur frequently!", + opt_inscribed_radius, min_obst_dist, costmap_inscribed_radius); +} + +void MpcLocalPlannerROS::customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg) +{ + std::lock_guard l(_custom_obst_mutex); + _custom_obstacle_msg = *obst_msg; +} + +void MpcLocalPlannerROS::customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg) +{ + ROS_INFO_ONCE("Via-points received. This message is printed once."); + if (_params.global_plan_viapoint_sep > 0) + { + ROS_WARN( + "Via-points are already obtained from the global plan (global_plan_viapoint_sep>0)." + "Ignoring custom via-points."); + _custom_via_points_active = false; + return; + } + + std::lock_guard lock(_via_point_mutex); + _via_points.clear(); + for (const geometry_msgs::PoseStamped& pose : via_points_msg->poses) + { + _via_points.emplace_back(pose.pose); + } + _custom_via_points_active = !_via_points.empty(); +} + +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromParamServer(const ros::NodeHandle& nh, + costmap_2d::Costmap2DROS* costmap_ros) +{ + std::string model_name; + if (!nh.getParam("footprint_model/type", model_name)) + { + ROS_INFO("No robot footprint model specified for trajectory optimization. Using point-shaped model."); + return boost::make_shared(); + } + + // from costmap_2d + if (model_name.compare("costmap_2d") == 0) + { + if (!costmap_ros) + { + ROS_WARN_STREAM("Costmap 2d pointer is null. Using point model instead."); + return boost::make_shared(); + } + ROS_INFO("Footprint model loaded from costmap_2d for trajectory optimization."); + return getRobotFootprintFromCostmap2d(*costmap_ros); + } + + // point + if (model_name.compare("point") == 0) + { + ROS_INFO("Footprint model 'point' loaded for trajectory optimization."); + return boost::make_shared(); + } + + // circular + if (model_name.compare("circular") == 0) + { + // get radius + double radius; + if (!nh.getParam("footprint_model/radius", radius)) + { + ROS_ERROR_STREAM("Footprint model 'circular' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() << "/footprint_model/radius' does not exist. Using point-model instead."); + return boost::make_shared(); + } + ROS_INFO_STREAM("Footprint model 'circular' (radius: " << radius << "m) loaded for trajectory optimization."); + return boost::make_shared(radius); + } + + // line + if (model_name.compare("line") == 0) + { + // check parameters + if (!nh.hasParam("footprint_model/line_start") || !nh.hasParam("footprint_model/line_end")) + { + ROS_ERROR_STREAM("Footprint model 'line' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() << "/footprint_model/line_start' and/or '.../line_end' do not exist. Using point-model instead."); + return boost::make_shared(); + } + // get line coordinates + std::vector line_start, line_end; + nh.getParam("footprint_model/line_start", line_start); + nh.getParam("footprint_model/line_end", line_end); + if (line_start.size() != 2 || line_end.size() != 2) + { + ROS_ERROR_STREAM( + "Footprint model 'line' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() + << "/footprint_model/line_start' and/or '.../line_end' do not contain x and y coordinates (2D). Using point-model instead."); + return boost::make_shared(); + } + + ROS_INFO_STREAM("Footprint model 'line' (line_start: [" << line_start[0] << "," << line_start[1] << "]m, line_end: [" << line_end[0] << "," + << line_end[1] << "]m) loaded for trajectory optimization."); + return boost::make_shared(Eigen::Map(line_start.data()), + Eigen::Map(line_end.data())); + } + + // two circles + if (model_name.compare("two_circles") == 0) + { + // check parameters + if (!nh.hasParam("footprint_model/front_offset") || !nh.hasParam("footprint_model/front_radius") || + !nh.hasParam("footprint_model/rear_offset") || !nh.hasParam("footprint_model/rear_radius")) + { + ROS_ERROR_STREAM("Footprint model 'two_circles' cannot be loaded for trajectory optimization, since params '" + << nh.getNamespace() + << "/footprint_model/front_offset', '.../front_radius', '.../rear_offset' and '.../rear_radius' do not exist. Using " + "point-model instead."); + return boost::make_shared(); + } + double front_offset, front_radius, rear_offset, rear_radius; + nh.getParam("footprint_model/front_offset", front_offset); + nh.getParam("footprint_model/front_radius", front_radius); + nh.getParam("footprint_model/rear_offset", rear_offset); + nh.getParam("footprint_model/rear_radius", rear_radius); + ROS_INFO_STREAM("Footprint model 'two_circles' (front_offset: " << front_offset << "m, front_radius: " << front_radius + << "m, rear_offset: " << rear_offset << "m, rear_radius: " << rear_radius + << "m) loaded for trajectory optimization."); + return boost::make_shared(front_offset, front_radius, rear_offset, rear_radius); + } + + // polygon + if (model_name.compare("polygon") == 0) + { + + // check parameters + XmlRpc::XmlRpcValue footprint_xmlrpc; + if (!nh.getParam("footprint_model/vertices", footprint_xmlrpc)) + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() << "/footprint_model/vertices' does not exist. Using point-model instead."); + return boost::make_shared(); + } + // get vertices + if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) + { + try + { + Point2dContainer polygon = makeFootprintFromXMLRPC(footprint_xmlrpc, "/footprint_model/vertices"); + ROS_INFO_STREAM("Footprint model 'polygon' loaded for trajectory optimization."); + return boost::make_shared(polygon); + } + catch (const std::exception& ex) + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization: " << ex.what() + << ". Using point-model instead."); + return boost::make_shared(); + } + } + else + { + ROS_ERROR_STREAM("Footprint model 'polygon' cannot be loaded for trajectory optimization, since param '" + << nh.getNamespace() + << "/footprint_model/vertices' does not define an array of coordinates. Using point-model instead."); + return boost::make_shared(); + } + } + + // otherwise + ROS_WARN_STREAM("Unknown robot footprint model specified with parameter '" << nh.getNamespace() + << "/footprint_model/type'. Using point model instead."); + return boost::make_shared(); +} + +teb_local_planner::RobotFootprintModelPtr MpcLocalPlannerROS::getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros) +{ + Point2dContainer footprint; + Eigen::Vector2d pt; + geometry_msgs::Polygon polygon = costmap_ros.getRobotFootprintPolygon(); + + for (int i = 0; i < polygon.points.size(); ++i) + { + pt.x() = polygon.points[i].x; + pt.y() = polygon.points[i].y; + + footprint.push_back(pt); + } + return boost::make_shared(footprint); +} + +teb_local_planner::Point2dContainer MpcLocalPlannerROS::makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, + const std::string& full_param_name) +{ + // Make sure we have an array of at least 3 elements. + if (footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray || footprint_xmlrpc.size() < 3) + { + ROS_FATAL("The footprint must be specified as list of lists on the parameter server, %s was specified as %s", full_param_name.c_str(), + std::string(footprint_xmlrpc).c_str()); + throw std::runtime_error( + "The footprint must be specified as list of lists on the parameter server with at least " + "3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]"); + } + + Point2dContainer footprint; + Eigen::Vector2d pt; + + for (int i = 0; i < footprint_xmlrpc.size(); ++i) + { + // Make sure each element of the list is an array of size 2. (x and y coordinates) + XmlRpc::XmlRpcValue point = footprint_xmlrpc[i]; + if (point.getType() != XmlRpc::XmlRpcValue::TypeArray || point.size() != 2) + { + ROS_FATAL( + "The footprint (parameter %s) must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.", + full_param_name.c_str()); + throw std::runtime_error( + "The footprint must be specified as list of lists on the parameter server eg: " + "[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form"); + } + + pt.x() = getNumberFromXMLRPC(point[0], full_param_name); + pt.y() = getNumberFromXMLRPC(point[1], full_param_name); + + footprint.push_back(pt); + } + return footprint; +} + +double MpcLocalPlannerROS::getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name) +{ + // Make sure that the value we're looking at is either a double or an int. + if (value.getType() != XmlRpc::XmlRpcValue::TypeInt && value.getType() != XmlRpc::XmlRpcValue::TypeDouble) + { + std::string& value_string = value; + ROS_FATAL("Values in the footprint specification (param %s) must be numbers. Found value %s.", full_param_name.c_str(), value_string.c_str()); + throw std::runtime_error("Values in the footprint specification must be numbers"); + } + return value.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(value) : (double)(value); +} + +} // end namespace mpc_local_planner diff --git a/mpc_local_planner_ros.h b/mpc_local_planner_ros.h new file mode 100644 index 0000000..e2f1a57 --- /dev/null +++ b/mpc_local_planner_ros.h @@ -0,0 +1,450 @@ +/********************************************************************* + * + * Software License Agreement + * + * Copyright (c) 2020, Christoph Rösmann, All rights reserved. + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * Authors: Christoph Rösmann + *********************************************************************/ + +#ifndef MPC_LOCAL_PLANNER_ROS_H_ +#define MPC_LOCAL_PLANNER_ROS_H_ + +#include + +// base local planner base class and utilities +#include +#include +#include +#include +#include + +// mpc_local_planner related classes +#include +#include + +// teb_local_planner related classes +#include +#include +#include + +// message types +#include +#include +#include +#include +#include +#include + +// transforms +#include +#include + +// costmap +#include +#include + +// dynamic reconfigure +// #include +// #include + +#include + +#include +#include + +namespace mpc_local_planner { + +/** + * @class MpcLocalPlannerROS + * @brief Implements both nav_core::BaseLocalPlanner and mbf_costmap_core::CostmapController abstract + * interfaces, so the teb_local_planner plugin can be used both in move_base and move_base_flex (MBF). + * @todo Escape behavior, more efficient obstacle handling + */ +class MpcLocalPlannerROS : public nav_core::BaseLocalPlanner, public mbf_costmap_core::CostmapController +{ + using PoseSE2 = teb_local_planner::PoseSE2; + using RobotFootprintModelPtr = teb_local_planner::RobotFootprintModelPtr; + using Point2dContainer = teb_local_planner::Point2dContainer; + using ObstContainer = teb_local_planner::ObstContainer; + using ViaPointContainer = std::vector; + + using ObstaclePtr = teb_local_planner::ObstaclePtr; + using PointObstacle = teb_local_planner::PointObstacle; + using CircularObstacle = teb_local_planner::CircularObstacle; + using LineObstacle = teb_local_planner::LineObstacle; + using PolygonObstacle = teb_local_planner::PolygonObstacle; + + public: + /** + * @brief Default constructor of the plugin + */ + MpcLocalPlannerROS(); + /** + * @brief Destructor of the plugin + */ + ~MpcLocalPlannerROS(); + + /** + * @brief Initializes the teb plugin + * @param name The name of the instance + * @param tf Pointer to a tf buffer + * @param costmap_ros Cost map representing occupied and free space + */ + + //void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros); + void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros); + + /** + * @brief Set the plan that the teb local planner is following + * @param orig_global_plan The plan to pass to the local planner + * @return True if the plan was updated successfully, false otherwise + */ + bool setPlan(const std::vector& orig_global_plan); + + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base + * @return True if a valid trajectory was found, false otherwise + */ + bool computeVelocityCommands(geometry_msgs::Twist& cmd_vel); + + /** + * @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base. + * @remark Extended version for MBF API + * @param pose the current pose of the robot. + * @param velocity the current velocity of the robot. + * @param cmd_vel Will be filled with the velocity command to be passed to the robot base. + * @param message Optional more detailed outcome as a string + * @return Result code as described on ExePath action result: + * SUCCESS = 0 + * 1..9 are reserved as plugin specific non-error results + * FAILURE = 100 Unspecified failure, only used for old, non-mfb_core based plugins + * CANCELED = 101 + * NO_VALID_CMD = 102 + * PAT_EXCEEDED = 103 + * COLLISION = 104 + * OSCILLATION = 105 + * ROBOT_STUCK = 106 + * MISSED_GOAL = 107 + * MISSED_PATH = 108 + * BLOCKED_PATH = 109 + * INVALID_PATH = 110 + * TF_ERROR = 111 + * NOT_INITIALIZED = 112 + * INVALID_PLUGIN = 113 + * INTERNAL_ERROR = 114 + * 121..149 are reserved as plugin specific errors + */ + uint32_t computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, + geometry_msgs::TwistStamped& cmd_vel, std::string& message); + + /** + * @brief Check if the goal pose has been achieved + * + * The actual check is performed in computeVelocityCommands(). + * Only the status flag is checked here. + * @return True if achieved, false otherwise + */ + bool isGoalReached(); + + /** + * @brief Dummy version to satisfy MBF API + */ + bool isGoalReached(double xy_tolerance, double yaw_tolerance) { return isGoalReached(); }; + + /** + * @brief Requests the planner to cancel, e.g. if it takes too much time + * @remark New on MBF API + * @return True if a cancel has been successfully requested, false if not implemented. + */ + bool cancel() { return false; }; + + /** @name Public utility functions/methods */ + //@{ + + /** + * @brief Transform a tf::Pose type into a Eigen::Vector2d containing the translational and angular velocities. + * + * Translational velocities (x- and y-coordinates) are combined into a single translational velocity (first component). + * @param tf_vel tf::Pose message containing a 1D or 2D translational velocity (x,y) and an angular velocity (yaw-angle) + * @return Translational and angular velocity combined into an Eigen::Vector2d + */ + static Eigen::Vector2d tfPoseToEigenVector2dTransRot(const tf::Pose& tf_vel); + + /** + * @brief Get the current robot footprint/contour model + * @param nh const reference to the local ros::NodeHandle + * @param costmap_ros pointer to an intialized instance of costmap_2d::Costmap2dROS + * @return Robot footprint model used for optimization + */ + static RobotFootprintModelPtr getRobotFootprintFromParamServer(const ros::NodeHandle& nh, costmap_2d::Costmap2DROS* costmap_ros = nullptr); + + /** + * @brief Get the current robot footprint/contour model + * @param costmap_ros reference to an intialized instance of costmap_2d::Costmap2dROS + * @return Robot footprint model used for optimization + */ + static RobotFootprintModelPtr getRobotFootprintFromCostmap2d(costmap_2d::Costmap2DROS& costmap_ros); + + /** + * @brief Set the footprint from the given XmlRpcValue. + * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros + * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point + * @param footprint_xmlrpc should be an array of arrays, where the top-level array should have 3 or more elements, and the + * sub-arrays should all have exactly 2 elements (x and y coordinates). + * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came. + * It is used only for reporting errors. + * @return container of vertices describing the polygon + */ + static Point2dContainer makeFootprintFromXMLRPC(XmlRpc::XmlRpcValue& footprint_xmlrpc, const std::string& full_param_name); + + /** + * @brief Get a number from the given XmlRpcValue. + * @remarks This method is copied from costmap_2d/footprint.h, since it is not declared public in all ros distros + * @remarks It is modified in order to return a container of Eigen::Vector2d instead of geometry_msgs::Point + * @param value double value type + * @param full_param_name this is the full name of the rosparam from which the footprint_xmlrpc value came. + * It is used only for reporting errors. + * @returns double value + */ + static double getNumberFromXMLRPC(XmlRpc::XmlRpcValue& value, const std::string& full_param_name); + + //@} + + protected: + /** + * @brief Update internal obstacle vector based on occupied costmap cells + * @remarks All occupied cells will be added as point obstacles. + * @remarks All previous obstacles are cleared. + * @sa updateObstacleContainerWithCostmapConverter + * @todo Include temporal coherence among obstacle msgs (id vector) + * @todo Include properties for dynamic obstacles (e.g. using constant velocity model) + */ + void updateObstacleContainerWithCostmap(); + + /** + * @brief Update internal obstacle vector based on polygons provided by a costmap_converter plugin + * @remarks Requires a loaded costmap_converter plugin. + * @remarks All previous obstacles are cleared. + * @sa updateObstacleContainerWithCostmap + */ + void updateObstacleContainerWithCostmapConverter(); + + /** + * @brief Update internal obstacle vector based on custom messages received via subscriber + * @remarks All previous obstacles are NOT cleared. Call this method after other update methods. + * @sa updateObstacleContainerWithCostmap, updateObstacleContainerWithCostmapConverter + */ + void updateObstacleContainerWithCustomObstacles(); + + /** + * @brief Update internal via-point container based on the current reference plan + * @remarks All previous via-points will be cleared. + * @param transformed_plan (local) portion of the global plan (which is already transformed to the planning frame) + * @param min_separation minimum separation between two consecutive via-points + */ + void updateViaPointsContainer(const std::vector& transformed_plan, double min_separation); + + /** + * @brief Callback for the dynamic_reconfigure node. + * + * This callback allows to modify parameters dynamically at runtime without restarting the node + * @param config Reference to the dynamic reconfigure config + * @param level Dynamic reconfigure level + */ + // void reconfigureCB(MpcLocalPlannerReconfigureConfig& config, uint32_t level); + + /** + * @brief Callback for custom obstacles that are not obtained from the costmap + * @param obst_msg pointer to the message containing a list of polygon shaped obstacles + */ + void customObstacleCB(const costmap_converter::ObstacleArrayMsg::ConstPtr& obst_msg); + + /** + * @brief Callback for custom via-points + * @param via_points_msg pointer to the message containing a list of via-points + */ + void customViaPointsCB(const nav_msgs::Path::ConstPtr& via_points_msg); + + /** + * @brief Prune global plan such that already passed poses are cut off + * + * The pose of the robot is transformed into the frame of the global plan by taking the most recent tf transform. + * If no valid transformation can be found, the method returns \c false. + * The global plan is pruned until the distance to the robot is at least \c dist_behind_robot. + * If no pose within the specified treshold \c dist_behind_robot can be found, + * nothing will be pruned and the method returns \c false. + * @remarks Do not choose \c dist_behind_robot too small (not smaller the cellsize of the map), otherwise nothing will be pruned. + * @param tf A reference to a tf buffer + * @param global_pose The global pose of the robot + * @param[in,out] global_plan The plan to be transformed + * @param dist_behind_robot Distance behind the robot that should be kept [meters] + * @return \c true if the plan is pruned, \c false in case of a transform exception or if no pose cannot be found inside the threshold + */ + + + //bool pruneGlobalPlan(const tf2_ros::Buffer& tf, const tf::Stamped& global_pose, + bool pruneGlobalPlan(const tf::TransformListener& tf, const tf::Stamped& global_pose, + std::vector& global_plan, double dist_behind_robot = 1); + + /** + * @brief Transforms the global plan of the robot from the planner frame to the local frame (modified). + * + * The method replaces transformGlobalPlan as defined in base_local_planner/goal_functions.h + * such that the index of the current goal pose is returned as well as + * the transformation between the global plan and the planning frame. + * @param tf A reference to a tf buffer + * @param global_plan The plan to be transformed + * @param global_pose The global pose of the robot + * @param costmap A reference to the costmap being used so the window size for transforming can be computed + * @param global_frame The frame to transform the plan to + * @param max_plan_length Specify maximum length (cumulative Euclidean distances) of the transformed plan [if <=0: disabled; the length is also + * bounded by the local costmap size!] + * @param[out] transformed_plan Populated with the transformed plan + * @param[out] current_goal_idx Index of the current (local) goal pose in the global plan + * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame + * @return \c true if the global plan is transformed, \c false otherwise + */ + //bool transformGlobalPlan(const tf2_ros::Buffer& tf, const std::vector& global_plan, + bool transformGlobalPlan(const tf::TransformListener& tf, const std::vector& global_plan, + const tf::Stamped& global_pose, const costmap_2d::Costmap2D& costmap, const std::string& global_frame, + double max_plan_length, std::vector& transformed_plan, int* current_goal_idx = NULL, + //geometry_msgs::TransformStamped* tf_plan_to_global = NULL) const; + tf::StampedTransform* tf_plan_to_global = NULL) const; + + /** + * @brief Estimate the orientation of a pose from the global_plan that is treated as a local goal for the local planner. + * + * If the current (local) goal point is not the final one (global) + * substitute the goal orientation by the angle of the direction vector between + * the local goal and the subsequent pose of the global plan. + * This is often helpful, if the global planner does not consider orientations. \n + * A moving average filter is utilized to smooth the orientation. + * @param global_plan The global plan + * @param local_goal Current local goal + * @param current_goal_idx Index of the current (local) goal pose in the global plan + * @param[out] tf_plan_to_global Transformation between the global plan and the global planning frame + * @param moving_average_length number of future poses of the global plan to be taken into account + * @return orientation (yaw-angle) estimate + */ + + //double estimateLocalGoalOrientation(const std::vector& global_plan, const geometry_msgs::PoseStamped& local_goal, + double estimateLocalGoalOrientation(const std::vector& global_plan, const tf::Stamped& local_goal, + int current_goal_idx, const tf::StampedTransform& tf_plan_to_global, + int moving_average_length = 3) const; + + /** + * @brief Validate current parameter values of the footprint for optimization, obstacle distance and the costmap footprint + * + * This method prints warnings if validation fails. + * @remarks Currently, we only validate the inscribed radius of the footprints + * @param opt_inscribed_radius Inscribed radius of the RobotFootprintModel for optimization + * @param costmap_inscribed_radius Inscribed radius of the footprint model used for the costmap + * @param min_obst_dist desired distance to obstacles + */ + void validateFootprints(double opt_inscribed_radius, double costmap_inscribed_radius, double min_obst_dist); + + private: + // Definition of member variables + + // external objects (store weak pointers) + costmap_2d::Costmap2DROS* _costmap_ros; //!< Pointer to the costmap ros wrapper, received from the navigation stack + costmap_2d::Costmap2D* _costmap; //!< Pointer to the 2d costmap (obtained from the costmap ros wrapper) + //tf2_ros::Buffer* _tf; //!< pointer to tf buffer + tf::TransformListener* _tf; + + // internal objects + Controller _controller; + ObstContainer _obstacles; //!< Obstacle vector that should be considered during local trajectory optimization + Publisher _publisher; + std::shared_ptr _costmap_model; + + corbo::TimeSeries::Ptr _x_seq = std::make_shared(); + corbo::TimeSeries::Ptr _u_seq = std::make_shared(); + + std::vector _global_plan; //!< Store the current global plan + + base_local_planner::OdometryHelperRos _odom_helper; //!< Provides an interface to receive the current velocity from the robot + + pluginlib::ClassLoader _costmap_converter_loader; //!< Load costmap converter plugins at runtime + boost::shared_ptr _costmap_converter; //!< Store the current costmap_converter + + // std::shared_ptr> + // dynamic_recfg_; //!< Dynamic reconfigure server to allow config modifications at runtime + ros::Subscriber _custom_obst_sub; //!< Subscriber for custom obstacles received via a ObstacleMsg. + std::mutex _custom_obst_mutex; //!< Mutex that locks the obstacle array (multi-threaded) + costmap_converter::ObstacleArrayMsg _custom_obstacle_msg; //!< Copy of the most recent obstacle message + + ViaPointContainer _via_points; + ros::Subscriber _via_points_sub; //!< Subscriber for custom via-points received via a Path msg. + bool _custom_via_points_active = false; //!< Keep track whether valid via-points have been received from via_points_sub_ + std::mutex _via_point_mutex; //!< Mutex that locks the via_points container (multi-threaded) + + PoseSE2 _robot_pose; //!< Store current robot pose + PoseSE2 _robot_goal; //!< Store current robot goal + geometry_msgs::Twist _robot_vel; //!< Store current robot translational and angular velocity (vx, vy, omega) + bool _goal_reached = false; //!< store whether the goal is reached or not + ros::Time _time_last_infeasible_plan; //!< Store at which time stamp the last infeasible plan was detected + int _no_infeasible_plans = 0; //!< Store how many times in a row the planner failed to find a feasible plan. + geometry_msgs::Twist _last_cmd; //!< Store the last control command generated in computeVelocityCommands() + ros::Time _time_last_cmd; + + RobotFootprintModelPtr _robot_model; + + std::vector _footprint_spec; //!< Store the footprint of the robot + double _robot_inscribed_radius; //!< The radius of the inscribed circle of the robot (collision possible) + double _robot_circumscribed_radius; //!< The radius of the circumscribed circle of the robot + + std::string _global_frame; //!< The frame in which the controller will run + std::string _robot_base_frame; //!< Used as the base frame id of the robot + + // flags + bool _initialized; //!< Keeps track about the correct initialization of this class + + struct Parameters + { + double xy_goal_tolerance = 0.2; + double yaw_goal_tolerance = 0.1; + bool global_plan_overwrite_orientation = true; + double global_plan_prune_distance = 1.0; + double max_global_plan_lookahead_dist = 1.5; + bool is_footprint_dynamic = false; + bool include_costmap_obstacles = true; + double costmap_obstacles_behind_robot_dist = 1.5; + double global_plan_viapoint_sep = -1; + double collision_check_min_resolution_angular = M_PI; + int collision_check_no_poses = -1; + std::string odom_topic = "odom"; + double controller_frequency = 10; + + } _params; + + struct CostmapConverterPlugin + { + std::string costmap_converter_plugin; + double costmap_converter_rate = 5; + bool costmap_converter_spin_thread = true; + + } _costmap_conv_params; + + public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +}; // end namespace mpc_local_planner + +#endif // MPC_LOCAL_PLANNER_ROS_H_