safe_trajectory_planner_ros.cpp
Go to the documentation of this file.
00001 /*
00002  * safe_trajectory_planner.cpp
00003  *
00004  *  Created on: Mar 25, 2010
00005  *      Author: duhadway
00006  */
00007 
00008 #include <safe_teleop_base/safe_trajectory_planner_ros.h>
00009 #include <ros/console.h>
00010 #include <sys/time.h>
00011 
00012 #include "geometry_msgs/PolygonStamped.h"
00013 #include "nav_msgs/Path.h"
00014 
00015 using namespace std;
00016 using namespace costmap_2d;
00017 using namespace base_local_planner;
00018 
00019 namespace safe_teleop {
00020 
00021   SafeTrajectoryPlannerROS::SafeTrajectoryPlannerROS(TFListener* tf, Costmap2DROS* costmap_ros)
00022     : nh_(), world_model_(NULL), tc_(NULL), costmap_ros_(costmap_ros), tf_(tf) {
00023     rot_stopped_velocity_ = 1e-2;
00024     trans_stopped_velocity_ = 1e-2;
00025     double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
00026     int vx_samples, vy_samples, vtheta_samples;
00027     double userdist_scale, occdist_scale;
00028     bool   dwa, holonomic_robot;
00029     double max_vel_x, min_vel_x;
00030     double max_vel_y, min_vel_y;
00031     string world_model_type;
00032 
00033     //initialize the copy of the costmap the controller will use
00034     //costmap_ros_->getCostmapCopy(costmap_);
00035     costmap_ = *costmap_ros_->getCostmap();
00036 
00037     ros::NodeHandle private_nh("~");
00038 
00039     l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
00040     u_plan_pub_ = private_nh.advertise<nav_msgs::Path>("user_plan", 1);
00041 
00042     global_frame_ = costmap_ros_->getGlobalFrameID();
00043     robot_base_frame_ = costmap_ros_->getBaseFrameID();
00044 
00045     odom_sub_ = nh_.subscribe<nav_msgs::Odometry>("/odom", 1, &SafeTrajectoryPlannerROS::odomCallback, this);
00046     user_sub_ = nh_.subscribe<geometry_msgs::Twist>("base_velocity", 1, boost::bind(&SafeTrajectoryPlannerROS::cmdCallback, this, _1));
00047 
00048     cmd_pub_ = private_nh.advertise<geometry_msgs::Twist>("safe_vel", 1);
00049 
00050     geometry_msgs::Twist vel;
00051     cmd_pub_.publish(vel);
00052 
00053     //we'll get the parameters for the robot radius from the costmap we're associated with
00054     inscribed_radius_ = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
00055     circumscribed_radius_ = costmap_ros_->getLayeredCostmap()->getCircumscribedRadius();
00056 
00057     private_nh.param("acc_lim_x", acc_lim_x, 2.5);
00058     private_nh.param("acc_lim_y", acc_lim_y, 2.5);
00059     private_nh.param("acc_lim_th", acc_lim_theta, 3.2);
00060     private_nh.param("sim_time", sim_time, 1.0);
00061     private_nh.param("sim_granularity", sim_granularity, 0.025);
00062     private_nh.param("vx_samples", vx_samples, 3);
00063     private_nh.param("vy_samples", vy_samples, 5);
00064     private_nh.param("vtheta_samples", vtheta_samples, 10);
00065     private_nh.param("user_bias", userdist_scale, 0.6);
00066     private_nh.param("occdist_scale", occdist_scale, 0.01);
00067     private_nh.param("max_vel_x", max_vel_x, 0.5);
00068     private_nh.param("min_vel_x", min_vel_x, 0.1);
00069     private_nh.param("max_vel_y", max_vel_y, 0.2);
00070     private_nh.param("min_vel_y", min_vel_y, -0.2);
00071 
00072 
00073     double max_rotational_vel;
00074     private_nh.param("max_rotational_vel", max_rotational_vel, 1.0);
00075     max_vel_th_ = max_rotational_vel;
00076     min_vel_th_ = -1.0 * max_rotational_vel;
00077 
00078     private_nh.param("world_model", world_model_type, string("costmap"));
00079     private_nh.param("holonomic_robot", holonomic_robot, true);
00080     private_nh.param("dwa", dwa, true);
00081 
00082     //parameters for using the freespace controller
00083     double min_pt_separation, max_obstacle_height, grid_resolution;
00084     private_nh.param("point_grid/max_sensor_range", max_sensor_range_, 2.0);
00085     private_nh.param("point_grid/min_pt_separation", min_pt_separation, 0.01);
00086     private_nh.param("point_grid/max_obstacle_height", max_obstacle_height, 2.0);
00087     private_nh.param("point_grid/grid_resolution", grid_resolution, 0.2);
00088 
00089     ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
00090     world_model_ = new CostmapModel(costmap_);
00091 
00092     std::vector<double> y_vels = loadYVels(private_nh);
00093 
00094     tc_ = new SafeTrajectoryPlanner(*world_model_, costmap_, costmap_ros_->getRobotFootprint(), inscribed_radius_, circumscribed_radius_,
00095         acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vy_samples, vtheta_samples,
00096         userdist_scale, occdist_scale,
00097         max_vel_x, min_vel_x, max_vel_y, min_vel_y, max_vel_th_, min_vel_th_,
00098         holonomic_robot, dwa);
00099   }
00100 
00101   std::vector<double> SafeTrajectoryPlannerROS::loadYVels(ros::NodeHandle node){
00102     std::vector<double> y_vels;
00103 
00104     XmlRpc::XmlRpcValue y_vel_list;
00105     if(node.getParam("y_vels", y_vel_list)){
00106       ROS_ASSERT_MSG(y_vel_list.getType() == XmlRpc::XmlRpcValue::TypeArray,
00107           "The y velocities to explore must be specified as a list");
00108 
00109       for(int i = 0; i < y_vel_list.size(); ++i){
00110         //make sure we have a list of lists of size 2
00111         XmlRpc::XmlRpcValue vel = y_vel_list[i];
00112 
00113         //make sure that the value we're looking at is either a double or an int
00114         ROS_ASSERT(vel.getType() == XmlRpc::XmlRpcValue::TypeInt || vel.getType() == XmlRpc::XmlRpcValue::TypeDouble);
00115         double y_vel = vel.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(vel) : (double)(vel);
00116 
00117         y_vels.push_back(y_vel);
00118 
00119       }
00120     }
00121     else{
00122       //if no values are passed in, we'll provide defaults
00123       y_vels.push_back(-0.3);
00124       y_vels.push_back(-0.1);
00125       y_vels.push_back(0.1);
00126       y_vels.push_back(0.3);
00127     }
00128 
00129     return y_vels;
00130   }
00131 
00132   SafeTrajectoryPlannerROS::~SafeTrajectoryPlannerROS(){
00133     if(tc_ != NULL)
00134       delete tc_;
00135 
00136     if(world_model_ != NULL)
00137       delete world_model_;
00138   }
00139 
00140   bool SafeTrajectoryPlannerROS::stopped(){
00141     boost::recursive_mutex::scoped_lock(odom_lock_);
00142     return abs(base_odom_.twist.twist.angular.z) <= rot_stopped_velocity_
00143       && abs(base_odom_.twist.twist.linear.x) <= trans_stopped_velocity_
00144       && abs(base_odom_.twist.twist.linear.y) <= trans_stopped_velocity_;
00145   }
00146 
00147   double SafeTrajectoryPlannerROS::distance(double x1, double y1, double x2, double y2){
00148     return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
00149   }
00150 
00151   void SafeTrajectoryPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
00152     //we assume that the odometry is published in the frame of the base
00153     boost::recursive_mutex::scoped_lock(odom_lock_);
00154     base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
00155     base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
00156     base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
00157 //    ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
00158 //        base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
00159   }
00160 
00161   void SafeTrajectoryPlannerROS::cmdCallback(const geometry_msgs::Twist::ConstPtr& vel) {
00162     if ((vel->linear.x > 0) || (fabs(vel->linear.y) > 0)) {
00163       geometry_msgs::Twist safe_vel;
00164       if (computeVelocityCommands(vel, safe_vel)) {
00165         cmd_pub_.publish(safe_vel);
00166         if ((vel->linear.x != safe_vel.linear.x) || (vel->angular.z != safe_vel.angular.z)) {
00167           ROS_DEBUG("safe: (%.2f, %.2f) -> (%.2f, %.2f)",
00168               vel->linear.x, vel->angular.z,
00169               safe_vel.linear.x, safe_vel.angular.z);
00170         }
00171       } else {
00172         geometry_msgs::Twist zero_vel;
00173         cmd_pub_.publish(zero_vel);
00174         ROS_DEBUG("zero: (%.2f, %.2f) -> (%.2f, %.2f)",
00175             vel->linear.x, vel->angular.z,
00176             zero_vel.linear.x, zero_vel.angular.z);
00177       }
00178 
00179     } else { // backwards and rotations allowed to pass through directly
00180       cmd_pub_.publish(vel);
00181     }
00182   }
00183 
00184   bool SafeTrajectoryPlannerROS::computeVelocityCommands(const geometry_msgs::Twist::ConstPtr& vel, geometry_msgs::Twist& cmd_vel){
00185     std::vector<geometry_msgs::PoseStamped> local_plan;
00186     std::vector<geometry_msgs::PoseStamped> user_plan;
00187     tf::Stamped<tf::Pose> global_pose;
00188 #if ROS_VERSION_MINIMUM(1, 14, 0) // ROS_MELODIC
00189     geometry_msgs::PoseStamped global_pose_stamped;
00190     if(!costmap_ros_->getRobotPose(global_pose_stamped))
00191       return false;
00192     tf::poseStampedMsgToTF(global_pose_stamped, global_pose);
00193 #else
00194     if(!costmap_ros_->getRobotPose(global_pose))
00195       return false;
00196 #endif
00197 
00198     //we also want to clear the robot footprint from the costmap we're using
00199 //    costmap_ros_->clearRobotFootprint();
00200 
00201     //make sure to update the costmap we'll use for this cycle
00202     costmap_ = *costmap_ros_->getCostmap();
00203 
00204     // Set current velocities from odometry
00205     geometry_msgs::Twist global_vel;
00206 
00207     odom_lock_.lock();
00208     global_vel.linear.x = base_odom_.twist.twist.linear.x;
00209     global_vel.linear.y = base_odom_.twist.twist.linear.y;
00210     global_vel.angular.z = base_odom_.twist.twist.angular.z;
00211     odom_lock_.unlock();
00212 
00213     tf::Stamped<tf::Pose> drive_cmds;
00214     drive_cmds.frame_id_ = robot_base_frame_;
00215 
00216     tf::Stamped<tf::Pose> robot_vel;
00217 
00218     robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
00219     robot_vel.frame_id_ = robot_base_frame_;
00220     robot_vel.stamp_ = ros::Time();
00221 
00222     tf::Stamped<tf::Pose> user_vel;
00223     user_vel.setData(tf::Transform(tf::createQuaternionFromYaw(vel->angular.z), tf::Vector3(vel->linear.x, vel->linear.y, 0)));
00224     user_vel.frame_id_ = robot_base_frame_;
00225     user_vel.stamp_ = ros::Time();
00226 
00227     /* For timing uncomment
00228     struct timeval start, end;
00229     double start_t, end_t, t_diff;
00230     gettimeofday(&start, NULL);
00231     */
00232 
00233     //compute what trajectory to drive along
00234     Trajectory path = tc_->findBestPath(global_pose, robot_vel, user_vel, drive_cmds);
00235     Trajectory user_path = tc_->findPath(global_pose, robot_vel, user_vel);
00236 
00237     /* For timing uncomment
00238     gettimeofday(&end, NULL);
00239     start_t = start.tv_sec + double(start.tv_usec) / 1e6;
00240     end_t = end.tv_sec + double(end.tv_usec) / 1e6;
00241     t_diff = end_t - start_t;
00242     ROS_INFO("Cycle time: %.9f", t_diff);
00243     */
00244 
00245     //pass along drive commands
00246     cmd_vel.linear.x = drive_cmds.getOrigin().getX();
00247     cmd_vel.linear.y = drive_cmds.getOrigin().getY();
00248     cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
00249 
00250     //if we cannot move... tell someone
00251     if(path.cost_ < 0){
00252       local_plan.clear();
00253 //      publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
00254       publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
00255       return false;
00256     }
00257 
00258     // Fill out the local plan
00259     for(unsigned int i = 0; i < path.getPointsSize(); ++i){
00260       double p_x, p_y, p_th;
00261       path.getPoint(i, p_x, p_y, p_th);
00262 
00263       tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::createQuaternionFromYaw(p_th), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), global_frame_);
00264       geometry_msgs::PoseStamped pose;
00265       tf::poseStampedTFToMsg(p, pose);
00266       local_plan.push_back(pose);
00267     }
00268 
00269     // Fill out the user plan
00270     for(unsigned int i = 0; i < user_path.getPointsSize(); ++i){
00271       double p_x, p_y, p_th;
00272       user_path.getPoint(i, p_x, p_y, p_th);
00273 
00274       tf::Stamped<tf::Pose> p = tf::Stamped<tf::Pose>(tf::Pose(tf::createQuaternionFromYaw(p_th), tf::Point(p_x, p_y, 0.0)), ros::Time::now(), global_frame_);
00275       geometry_msgs::PoseStamped pose;
00276       tf::poseStampedTFToMsg(p, pose);
00277       user_plan.push_back(pose);
00278     }
00279 
00280     //publish information to the visualizer
00281     publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
00282     publishPlan(user_plan, u_plan_pub_, 0.0, 0.0, 1.0, 0.0);
00283     return true;
00284   }
00285 
00286   void SafeTrajectoryPlannerROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a){
00287     //given an empty path we won't do anything
00288     if(path.empty())
00289       return;
00290 
00291     //create a path message
00292     nav_msgs::Path gui_path;
00293     gui_path.poses.resize(path.size());
00294     gui_path.header.frame_id = global_frame_;
00295     gui_path.header.stamp = path[0].header.stamp;
00296 
00297     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
00298     for(unsigned int i=0; i < path.size(); i++){
00299       gui_path.poses[i] = path[i];
00300     }
00301 
00302     pub.publish(gui_path);
00303   }
00304 };
00305 
00306 int main(int argc, char** argv) {
00307   ros::init(argc, argv, "safe_trajectory_node");
00308 #if ROS_VERSION_MINIMUM(1, 14, 0) // ROS_MELODIC
00309   safe_teleop::TFListener tf;
00310   tf2_ros::TransformListener tf2_listener(tf);
00311 #else
00312   safe_teleop::TFListener tf(ros::Duration(10));
00313 #endif
00314   costmap_2d::Costmap2DROS* costmap_ros = new costmap_2d::Costmap2DROS("local_costmap", tf);
00315   safe_teleop::SafeTrajectoryPlannerROS* planner = new safe_teleop::SafeTrajectoryPlannerROS(&tf, costmap_ros);
00316 
00317   ros::spin();
00318 
00319   delete planner;
00320   delete costmap_ros;
00321   return(0);
00322 }


safe_teleop_base
Author(s):
autogenerated on Thu Mar 14 2019 02:49:46