00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00038 #include <twist_recovery/twist_recovery.h>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <tf/transform_datatypes.h>
00041
00042
00043 PLUGINLIB_DECLARE_CLASS(twist_recovery, TwistRecovery, twist_recovery::TwistRecovery,
00044 nav_core::RecoveryBehavior)
00045
00046 namespace gm=geometry_msgs;
00047 namespace cmap=costmap_2d;
00048 namespace blp=base_local_planner;
00049 using std::vector;
00050 using std::max;
00051
00052 namespace twist_recovery
00053 {
00054
00055 TwistRecovery::TwistRecovery () :
00056 global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
00057 {}
00058
00059 TwistRecovery::~TwistRecovery ()
00060 {
00061 delete world_model_;
00062 }
00063
00064 void TwistRecovery::initialize (std::string name, tf::TransformListener* tf,
00065 cmap::Costmap2DROS* global_cmap, cmap::Costmap2DROS* local_cmap)
00066 {
00067 ROS_ASSERT(!initialized_);
00068 name_ = name;
00069 tf_ = tf;
00070 local_costmap_ = local_cmap;
00071 global_costmap_ = global_cmap;
00072 local_costmap_->getCostmapCopy(costmap_);
00073 world_model_ = new blp::CostmapModel(costmap_);
00074
00075 pub_ = nh_.advertise<gm::Twist>("cmd_vel", 10);
00076 ros::NodeHandle private_nh("~/" + name);
00077
00078 {
00079 bool found=true;
00080 found = found && private_nh.getParam("linear_x", base_frame_twist_.linear.x);
00081 found = found && private_nh.getParam("linear_y", base_frame_twist_.linear.y);
00082 found = found && private_nh.getParam("angular_z", base_frame_twist_.angular.z);
00083 if (!found) {
00084 ROS_FATAL_STREAM ("Didn't find twist parameters in " << private_nh.getNamespace());
00085 ros::shutdown();
00086 }
00087 }
00088
00089 private_nh.param("duration", duration_, 1.0);
00090 private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
00091 private_nh.param("angular_speed_limit", angular_speed_limit_, 1.0);
00092 private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
00093 private_nh.param("angular_acceleration_limit", angular_acceleration_limit_, 3.2);
00094 private_nh.param("controller_frequency", controller_frequency_, 20.0);
00095 private_nh.param("simulation_inc", simulation_inc_, 1/controller_frequency_);
00096
00097 ROS_INFO_STREAM_NAMED ("top", "Initialized twist recovery with twist " <<
00098 base_frame_twist_ << " and duration " << duration_);
00099
00100 initialized_ = true;
00101 }
00102
00103 gm::Twist scaleTwist (const gm::Twist& twist, const double scale)
00104 {
00105 gm::Twist t;
00106 t.linear.x = twist.linear.x * scale;
00107 t.linear.y = twist.linear.y * scale;
00108 t.angular.z = twist.angular.z * scale;
00109 return t;
00110 }
00111
00112 gm::Pose2D forwardSimulate (const gm::Pose2D& p, const gm::Twist& twist, const double t=1.0)
00113 {
00114 gm::Pose2D p2;
00115 p2.x = p.x + twist.linear.x*t;
00116 p2.y = p.y + twist.linear.y*t;
00117 p2.theta = p.theta + twist.angular.z*t;
00118 return p2;
00119 }
00120
00122 double TwistRecovery::normalizedPoseCost (const gm::Pose2D& pose) const
00123 {
00124 gm::Point p;
00125 p.x = pose.x;
00126 p.y = pose.y;
00127 vector<gm::Point> oriented_footprint;
00128 local_costmap_->getOrientedFootprint(pose.x, pose.y, pose.theta, oriented_footprint);
00129 const double c = world_model_->footprintCost(p, oriented_footprint, local_costmap_->getInscribedRadius(),
00130 local_costmap_->getCircumscribedRadius());
00131 return c < 0 ? 1e9 : c;
00132 }
00133
00134
00139 double TwistRecovery::nonincreasingCostInterval (const gm::Pose2D& current, const gm::Twist& twist) const
00140 {
00141 double cost = normalizedPoseCost(current);
00142 double t;
00143 for (t=simulation_inc_; t<=duration_; t+=simulation_inc_) {
00144 const double next_cost = normalizedPoseCost(forwardSimulate(current, twist, t));
00145 if (next_cost > cost) {
00146 ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
00147 << " is " << next_cost << " which is greater than previous cost " << cost);
00148 break;
00149 }
00150 cost = next_cost;
00151 }
00152
00153 return t-simulation_inc_;
00154 }
00155
00156 double linearSpeed (const gm::Twist& twist)
00157 {
00158 return sqrt(twist.linear.x*twist.linear.x + twist.linear.y*twist.linear.y);
00159 }
00160
00161 double angularSpeed (const gm::Twist& twist)
00162 {
00163 return fabs(twist.angular.z);
00164 }
00165
00166
00167 gm::Twist TwistRecovery::scaleGivenAccelerationLimits (const gm::Twist& twist, const double time_remaining) const
00168 {
00169 const double linear_speed = linearSpeed(twist);
00170 const double angular_speed = angularSpeed(twist);
00171 const double linear_acc_scaling = linear_speed/(time_remaining*linear_acceleration_limit_);
00172 const double angular_acc_scaling = angular_speed/(time_remaining*angular_acceleration_limit_);
00173 const double acc_scaling = max(linear_acc_scaling, angular_acc_scaling);
00174 const double linear_vel_scaling = linear_speed/linear_speed_limit_;
00175 const double angular_vel_scaling = angular_speed/angular_speed_limit_;
00176 const double vel_scaling = max(linear_vel_scaling, angular_vel_scaling);
00177 return scaleTwist(twist, max(1.0, max(acc_scaling, vel_scaling)));
00178 }
00179
00180
00181 gm::Pose2D TwistRecovery::getCurrentLocalPose () const
00182 {
00183 tf::Stamped<tf::Pose> p;
00184 local_costmap_->getRobotPose(p);
00185 gm::Pose2D pose;
00186 pose.x = p.getOrigin().x();
00187 pose.y = p.getOrigin().y();
00188 pose.theta = tf::getYaw(p.getRotation());
00189 return pose;
00190 }
00191
00192 void TwistRecovery::runBehavior ()
00193 {
00194 ROS_ASSERT (initialized_);
00195
00196
00197 const gm::Pose2D& current = getCurrentLocalPose();
00198 local_costmap_->getCostmapCopy(costmap_);
00199
00200 const double d = nonincreasingCostInterval(current, base_frame_twist_);
00201 ros::Rate r(controller_frequency_);
00202 ROS_INFO_NAMED ("top", "Applying (%.2f, %.2f, %.2f) for %.2f seconds", base_frame_twist_.linear.x,
00203 base_frame_twist_.linear.y, base_frame_twist_.angular.z, d);
00204
00205
00206 for (double t=0; t<d; t+=1/controller_frequency_) {
00207 pub_.publish(scaleGivenAccelerationLimits(base_frame_twist_, d-t));
00208 r.sleep();
00209 }
00210 }
00211
00212
00213 }
00214