safe_trajectory_planner_ros.cpp
Go to the documentation of this file.
1 /*
2  * safe_trajectory_planner.cpp
3  *
4  * Created on: Mar 25, 2010
5  * Author: duhadway
6  */
7 
9 #include <ros/console.h>
10 #include <sys/time.h>
11 
12 #include "geometry_msgs/PolygonStamped.h"
13 #include "nav_msgs/Path.h"
14 
15 using namespace std;
16 using namespace costmap_2d;
17 using namespace base_local_planner;
18 
19 namespace safe_teleop {
20 
21  SafeTrajectoryPlannerROS::SafeTrajectoryPlannerROS(TFListener* tf, Costmap2DROS* costmap_ros)
22  : nh_(), world_model_(NULL), tc_(NULL), costmap_ros_(costmap_ros), tf_(tf) {
23  rot_stopped_velocity_ = 1e-2;
25  double acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity;
26  int vx_samples, vy_samples, vtheta_samples;
27  double userdist_scale, occdist_scale;
28  bool dwa, holonomic_robot;
29  double max_vel_x, min_vel_x;
30  double max_vel_y, min_vel_y;
31  string world_model_type;
32 
33  //initialize the copy of the costmap the controller will use
34  //costmap_ros_->getCostmapCopy(costmap_);
36 
37  ros::NodeHandle private_nh("~");
38 
39  l_plan_pub_ = private_nh.advertise<nav_msgs::Path>("local_plan", 1);
40  u_plan_pub_ = private_nh.advertise<nav_msgs::Path>("user_plan", 1);
41 
44 
45  odom_sub_ = nh_.subscribe<nav_msgs::Odometry>("/odom", 1, &SafeTrajectoryPlannerROS::odomCallback, this);
46  user_sub_ = nh_.subscribe<geometry_msgs::Twist>("base_velocity", 1, boost::bind(&SafeTrajectoryPlannerROS::cmdCallback, this, _1));
47 
48  cmd_pub_ = private_nh.advertise<geometry_msgs::Twist>("safe_vel", 1);
49 
50  geometry_msgs::Twist vel;
51  cmd_pub_.publish(vel);
52 
54 
55  //we'll get the parameters for the robot radius from the costmap we're associated with
58 
59  private_nh.param("acc_lim_x", acc_lim_x, 2.5);
60  private_nh.param("acc_lim_y", acc_lim_y, 2.5);
61  private_nh.param("acc_lim_th", acc_lim_theta, 3.2);
62  private_nh.param("sim_time", sim_time, 1.0);
63  private_nh.param("sim_granularity", sim_granularity, 0.025);
64  private_nh.param("vx_samples", vx_samples, 3);
65  private_nh.param("vy_samples", vy_samples, 5);
66  private_nh.param("vtheta_samples", vtheta_samples, 10);
67  private_nh.param("user_bias", userdist_scale, 0.6);
68  private_nh.param("occdist_scale", occdist_scale, 0.01);
69  private_nh.param("max_vel_x", max_vel_x, 0.5);
70  private_nh.param("min_vel_x", min_vel_x, 0.1);
71  private_nh.param("max_vel_y", max_vel_y, 0.2);
72  private_nh.param("min_vel_y", min_vel_y, -0.2);
73 
74 
75  double max_rotational_vel;
76  private_nh.param("max_rotational_vel", max_rotational_vel, 1.0);
77  max_vel_th_ = max_rotational_vel;
78  min_vel_th_ = -1.0 * max_rotational_vel;
79 
80  private_nh.param("world_model", world_model_type, string("costmap"));
81  private_nh.param("holonomic_robot", holonomic_robot, true);
82  private_nh.param("dwa", dwa, true);
83  private_nh.param("safe_backwards", safe_backwards_, false);
84 
85  //parameters for using the freespace controller
86  double min_pt_separation, max_obstacle_height, grid_resolution;
87  private_nh.param("point_grid/max_sensor_range", max_sensor_range_, 2.0);
88  private_nh.param("point_grid/min_pt_separation", min_pt_separation, 0.01);
89  private_nh.param("point_grid/max_obstacle_height", max_obstacle_height, 2.0);
90  private_nh.param("point_grid/grid_resolution", grid_resolution, 0.2);
91 
92  ROS_ASSERT_MSG(world_model_type == "costmap", "At this time, only costmap world models are supported by this controller");
94 
95  std::vector<double> y_vels = loadYVels(private_nh);
96 
98  acc_lim_x, acc_lim_y, acc_lim_theta, sim_time, sim_granularity, vx_samples, vy_samples, vtheta_samples,
99  userdist_scale, occdist_scale,
100  max_vel_x, min_vel_x, max_vel_y, min_vel_y, max_vel_th_, min_vel_th_,
101  holonomic_robot, dwa);
102  }
103 
105  std::vector<double> y_vels;
106 
107  XmlRpc::XmlRpcValue y_vel_list;
108  if(node.getParam("y_vels", y_vel_list)){
110  "The y velocities to explore must be specified as a list");
111 
112  for(int i = 0; i < y_vel_list.size(); ++i){
113  //make sure we have a list of lists of size 2
114  XmlRpc::XmlRpcValue vel = y_vel_list[i];
115 
116  //make sure that the value we're looking at is either a double or an int
118  double y_vel = vel.getType() == XmlRpc::XmlRpcValue::TypeInt ? (int)(vel) : (double)(vel);
119 
120  y_vels.push_back(y_vel);
121 
122  }
123  }
124  else{
125  //if no values are passed in, we'll provide defaults
126  y_vels.push_back(-0.3);
127  y_vels.push_back(-0.1);
128  y_vels.push_back(0.1);
129  y_vels.push_back(0.3);
130  }
131 
132  return y_vels;
133  }
134 
136  if(tc_ != NULL)
137  delete tc_;
138 
139  if(world_model_ != NULL)
140  delete world_model_;
141  }
142 
144  boost::recursive_mutex::scoped_lock(odom_lock_);
145  return abs(base_odom_.twist.twist.angular.z) <= rot_stopped_velocity_
146  && abs(base_odom_.twist.twist.linear.x) <= trans_stopped_velocity_
147  && abs(base_odom_.twist.twist.linear.y) <= trans_stopped_velocity_;
148  }
149 
150  double SafeTrajectoryPlannerROS::distance(double x1, double y1, double x2, double y2){
151  return sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
152  }
153 
154  void SafeTrajectoryPlannerROS::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
155  //we assume that the odometry is published in the frame of the base
156  boost::recursive_mutex::scoped_lock(odom_lock_);
157  base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
158  base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
159  base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
160 // ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
161 // base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
162  }
163 
164  void SafeTrajectoryPlannerROS::cmdCallback(const geometry_msgs::Twist::ConstPtr& vel) {
165  if ((safe_backwards_ && ((fabs(vel->linear.x) > 0) || (fabs(vel->linear.y) > 0))) ||
166  (!safe_backwards_ && ((vel->linear.x > 0) || (fabs(vel->linear.y) > 0))))
167  {
168  geometry_msgs::Twist safe_vel;
169  if (computeVelocityCommands(vel, safe_vel)) {
170  cmd_pub_.publish(safe_vel);
171  if ((vel->linear.x != safe_vel.linear.x) || (vel->angular.z != safe_vel.angular.z)) {
172  ROS_DEBUG("safe: (%.2f, %.2f) -> (%.2f, %.2f)",
173  vel->linear.x, vel->angular.z,
174  safe_vel.linear.x, safe_vel.angular.z);
175  }
176  } else {
177  geometry_msgs::Twist zero_vel;
178  cmd_pub_.publish(zero_vel);
179  ROS_DEBUG("zero: (%.2f, %.2f) -> (%.2f, %.2f)",
180  vel->linear.x, vel->angular.z,
181  zero_vel.linear.x, zero_vel.angular.z);
182  }
183 
184  } else { // backwards and rotations allowed to pass through directly
185  cmd_pub_.publish(vel);
186  }
187  }
188 
189  bool SafeTrajectoryPlannerROS::computeVelocityCommands(const geometry_msgs::Twist::ConstPtr& vel, geometry_msgs::Twist& cmd_vel){
190  std::vector<geometry_msgs::PoseStamped> local_plan;
191  std::vector<geometry_msgs::PoseStamped> user_plan;
192  tf::Stamped<tf::Pose> global_pose;
193 #if ROS_VERSION_MINIMUM(1, 14, 0) // ROS_MELODIC
194  geometry_msgs::PoseStamped global_pose_stamped;
195  if(!costmap_ros_->getRobotPose(global_pose_stamped))
196  return false;
197  tf::poseStampedMsgToTF(global_pose_stamped, global_pose);
198 #else
199  if(!costmap_ros_->getRobotPose(global_pose))
200  return false;
201 #endif
202 
203  //we also want to clear the robot footprint from the costmap we're using
204 // costmap_ros_->clearRobotFootprint();
205 
206  //make sure to update the costmap we'll use for this cycle
208 
209  // Set current velocities from odometry
210  geometry_msgs::Twist global_vel;
211 
212  odom_lock_.lock();
213  global_vel.linear.x = base_odom_.twist.twist.linear.x;
214  global_vel.linear.y = base_odom_.twist.twist.linear.y;
215  global_vel.angular.z = base_odom_.twist.twist.angular.z;
216  odom_lock_.unlock();
217 
218  tf::Stamped<tf::Pose> drive_cmds;
219  drive_cmds.frame_id_ = robot_base_frame_;
220 
221  tf::Stamped<tf::Pose> robot_vel;
222 
223  robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
224  robot_vel.frame_id_ = robot_base_frame_;
225  robot_vel.stamp_ = ros::Time();
226 
227  tf::Stamped<tf::Pose> user_vel;
228  user_vel.setData(tf::Transform(tf::createQuaternionFromYaw(vel->angular.z), tf::Vector3(vel->linear.x, vel->linear.y, 0)));
229  user_vel.frame_id_ = robot_base_frame_;
230  user_vel.stamp_ = ros::Time();
231 
232  /* For timing uncomment
233  struct timeval start, end;
234  double start_t, end_t, t_diff;
235  gettimeofday(&start, NULL);
236  */
237 
238  //compute what trajectory to drive along
239  Trajectory path = tc_->findBestPath(global_pose, robot_vel, user_vel, drive_cmds);
240  Trajectory user_path = tc_->findPath(global_pose, robot_vel, user_vel);
241 
242  /* For timing uncomment
243  gettimeofday(&end, NULL);
244  start_t = start.tv_sec + double(start.tv_usec) / 1e6;
245  end_t = end.tv_sec + double(end.tv_usec) / 1e6;
246  t_diff = end_t - start_t;
247  ROS_INFO("Cycle time: %.9f", t_diff);
248  */
249 
250  //pass along drive commands
251  cmd_vel.linear.x = drive_cmds.getOrigin().getX();
252  cmd_vel.linear.y = drive_cmds.getOrigin().getY();
253  cmd_vel.angular.z = tf::getYaw(drive_cmds.getRotation());
254 
255  //if we cannot move... tell someone
256  if(path.cost_ < 0){
257  local_plan.clear();
258 // publishPlan(transformed_plan, g_plan_pub_, 0.0, 1.0, 0.0, 0.0);
259  publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
260  return false;
261  }
262 
263  // Fill out the local plan
264  for(unsigned int i = 0; i < path.getPointsSize(); ++i){
265  double p_x, p_y, p_th;
266  path.getPoint(i, p_x, p_y, p_th);
267 
269  geometry_msgs::PoseStamped pose;
270  tf::poseStampedTFToMsg(p, pose);
271  local_plan.push_back(pose);
272  }
273 
274  // Fill out the user plan
275  for(unsigned int i = 0; i < user_path.getPointsSize(); ++i){
276  double p_x, p_y, p_th;
277  user_path.getPoint(i, p_x, p_y, p_th);
278 
280  geometry_msgs::PoseStamped pose;
281  tf::poseStampedTFToMsg(p, pose);
282  user_plan.push_back(pose);
283  }
284 
285  //publish information to the visualizer
286  publishPlan(local_plan, l_plan_pub_, 0.0, 0.0, 1.0, 0.0);
287  publishPlan(user_plan, u_plan_pub_, 0.0, 0.0, 1.0, 0.0);
288  return true;
289  }
290 
291  void SafeTrajectoryPlannerROS::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path, const ros::Publisher& pub, double r, double g, double b, double a){
292  //given an empty path we won't do anything
293  if(path.empty())
294  return;
295 
296  //create a path message
297  nav_msgs::Path gui_path;
298  gui_path.poses.resize(path.size());
299  gui_path.header.frame_id = global_frame_;
300  gui_path.header.stamp = path[0].header.stamp;
301 
302  // Extract the plan in world co-ordinates, we assume the path is all in the same frame
303  for(unsigned int i=0; i < path.size(); i++){
304  gui_path.poses[i] = path[i];
305  }
306 
307  pub.publish(gui_path);
308  }
309 
310  bool SafeTrajectoryPlannerROS::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
312  return true;
313  }
314 };
315 
316 int main(int argc, char** argv) {
317  ros::init(argc, argv, "safe_trajectory_node");
318 #if ROS_VERSION_MINIMUM(1, 14, 0) // ROS_MELODIC
320  tf2_ros::TransformListener tf2_listener(tf);
321 #else
323 #endif
324  costmap_2d::Costmap2DROS* costmap_ros = new costmap_2d::Costmap2DROS("local_costmap", tf);
326 
327  ros::spin();
328 
329  delete planner;
330  delete costmap_ros;
331  return(0);
332 }
int main(int argc, char **argv)
void publish(const boost::shared_ptr< M > &message) const
void setData(const T &input)
double distance(double x1, double y1, double x2, double y2)
Compute the distance between two points.
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int size() const
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
static double getYaw(const Quaternion &bt_q)
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub, double r, double g, double b, double a)
Publish a plan for visualization purposes.
std::string getGlobalFrameID()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double max_sensor_range_
Keep track of the effective maximum range of our sensors.
costmap_2d::Costmap2D costmap_
The costmap the controller will use.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void getPoint(unsigned int index, double &x, double &y, double &th) const
std::string robot_base_frame_
Used as the base frame id of the robot.
tf::TransformListener * tf_
Type const & getType() const
costmap_2d::Costmap2DROS * costmap_ros_
The ROS wrapper for the costmap the controller will use.
ROSCPP_DECL void spin(Spinner &spinner)
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
Computes control velocities for a robot given a costmap, a plan, and the robot&#39;s position in the worl...
tf::Transform Pose
void cmdCallback(const geometry_msgs::Twist::ConstPtr &vel)
nav_msgs::Odometry base_odom_
Used to get the velocity of the robot.
static Quaternion createQuaternionFromYaw(double yaw)
base_local_planner::Trajectory findPath(tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > user_vel)
Given the current position, orientation, and velocity of the robot, return a trajectory to follow...
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define ROS_ASSERT_MSG(cond,...)
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp)
std::string getBaseFrameID()
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string global_frame_
The frame in which the controller will run.
A ROS wrapper for the trajectory controller that queries the param server to construct a controller...
unsigned int getPointsSize() const
bool stopped()
Check whether the robot is stopped or not.
~SafeTrajectoryPlannerROS()
Destructor for the wrapper.
std::vector< double > loadYVels(ros::NodeHandle node)
ros::Time stamp_
std::string frame_id_
bool getParam(const std::string &key, std::string &s) const
static Time now()
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
bool computeVelocityCommands(const geometry_msgs::Twist::ConstPtr &vel, geometry_msgs::Twist &cmd_vel)
Given the current position, orientation, and velocity of the robot, compute velocity commands to send...
Costmap2D * getCostmap()
#define ROS_ASSERT(cond)
base_local_planner::Trajectory findBestPath(tf::Stamped< tf::Pose > global_pose, tf::Stamped< tf::Pose > global_vel, tf::Stamped< tf::Pose > user_vel, tf::Stamped< tf::Pose > &drive_velocities)
Given the current position, orientation, and velocity of the robot, return a trajectory to follow...
std::vector< geometry_msgs::Point > getRobotFootprint()
base_local_planner::WorldModel * world_model_
The world model that the controller will use.
SafeTrajectoryPlanner * tc_
The trajectory controller.
LayeredCostmap * getLayeredCostmap()
#define ROS_DEBUG(...)


safe_teleop_base
Author(s):
autogenerated on Fri Dec 20 2019 04:00:23