twist_recovery.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  */
30 
40 #include <tf/transform_datatypes.h>
41 
42 // register as a RecoveryBehavior plugin
44 
45 namespace gm=geometry_msgs;
46 namespace cmap=costmap_2d;
47 namespace blp=base_local_planner;
48 using std::vector;
49 using std::max;
50 
51 namespace twist_recovery
52 {
53 
54 TwistRecovery::TwistRecovery () :
55  global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
56 {}
57 
59 {
60  delete world_model_;
61 }
62 
64  cmap::Costmap2DROS* global_cmap, cmap::Costmap2DROS* local_cmap)
65 {
67  name_ = name;
68  tf_ = tf;
69  local_costmap_ = local_cmap;
70  global_costmap_ = global_cmap;
72 
73  pub_ = nh_.advertise<gm::Twist>("cmd_vel", 10);
74  ros::NodeHandle private_nh("~/" + name);
75 
76  {
77  bool found=true;
78  found = found && private_nh.getParam("linear_x", base_frame_twist_.linear.x);
79  found = found && private_nh.getParam("linear_y", base_frame_twist_.linear.y);
80  found = found && private_nh.getParam("angular_z", base_frame_twist_.angular.z);
81  if (!found) {
82  ROS_FATAL_STREAM ("Didn't find twist parameters in " << private_nh.getNamespace());
83  ros::shutdown();
84  }
85  }
86 
87  private_nh.param("duration", duration_, 1.0);
88  private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
89  private_nh.param("angular_speed_limit", angular_speed_limit_, 1.0);
90  private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
91  private_nh.param("angular_acceleration_limit", angular_acceleration_limit_, 3.2);
92  private_nh.param("controller_frequency", controller_frequency_, 20.0);
93  private_nh.param("simulation_inc", simulation_inc_, 1/controller_frequency_);
94 
95  ROS_INFO_STREAM_NAMED ("top", "Initialized twist recovery with twist " <<
96  base_frame_twist_ << " and duration " << duration_);
97 
98  initialized_ = true;
99 }
100 
101 gm::Twist scaleTwist (const gm::Twist& twist, const double scale)
102 {
103  gm::Twist t;
104  t.linear.x = twist.linear.x * scale;
105  t.linear.y = twist.linear.y * scale;
106  t.angular.z = twist.angular.z * scale;
107  return t;
108 }
109 
110 gm::Pose2D forwardSimulate (const gm::Pose2D& p, const gm::Twist& twist, const double t=1.0)
111 {
112  gm::Pose2D p2;
113  p2.x = p.x + twist.linear.x*t;
114  p2.y = p.y + twist.linear.y*t;
115  p2.theta = p.theta + twist.angular.z*t;
116  return p2;
117 }
118 
120 double TwistRecovery::normalizedPoseCost (const gm::Pose2D& pose) const
121 {
122  const double c = world_model_->footprintCost(pose.x, pose.y, pose.theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
123  return c < 0 ? 1e9 : c;
124 }
125 
126 
131 double TwistRecovery::nonincreasingCostInterval (const gm::Pose2D& current, const gm::Twist& twist) const
132 {
133  double cost = normalizedPoseCost(current);
134  double t; // Will hold the first time that is invalid
135  for (t=simulation_inc_; t<=duration_; t+=simulation_inc_) {
136  const double next_cost = normalizedPoseCost(forwardSimulate(current, twist, t));
137  if (next_cost > cost) {
138  ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
139  << " is " << next_cost << " which is greater than previous cost " << cost);
140  break;
141  }
142  cost = next_cost;
143  }
144 
145  return t-simulation_inc_;
146 }
147 
148 double linearSpeed (const gm::Twist& twist)
149 {
150  return sqrt(twist.linear.x*twist.linear.x + twist.linear.y*twist.linear.y);
151 }
152 
153 double angularSpeed (const gm::Twist& twist)
154 {
155  return fabs(twist.angular.z);
156 }
157 
158 // Scale twist so we can stop in the given time, and so it's within the max velocity
159 gm::Twist TwistRecovery::scaleGivenAccelerationLimits (const gm::Twist& twist, const double time_remaining) const
160 {
161  const double linear_speed = linearSpeed(twist);
162  const double angular_speed = angularSpeed(twist);
163  const double linear_acc_scaling = linear_speed/(time_remaining*linear_acceleration_limit_);
164  const double angular_acc_scaling = angular_speed/(time_remaining*angular_acceleration_limit_);
165  const double acc_scaling = max(linear_acc_scaling, angular_acc_scaling);
166  const double linear_vel_scaling = linear_speed/linear_speed_limit_;
167  const double angular_vel_scaling = angular_speed/angular_speed_limit_;
168  const double vel_scaling = max(linear_vel_scaling, angular_vel_scaling);
169  return scaleTwist(twist, max(1.0, max(acc_scaling, vel_scaling)));
170 }
171 
172 // Get pose in local costmap frame
174 {
177  gm::Pose2D pose;
178  pose.x = p.getOrigin().x();
179  pose.y = p.getOrigin().y();
180  pose.theta = tf::getYaw(p.getRotation());
181  return pose;
182 }
183 
185 {
187 
188  // Figure out how long we can safely run the behavior
189  const gm::Pose2D& current = getCurrentLocalPose();
190 
191  const double d = nonincreasingCostInterval(current, base_frame_twist_);
193  ROS_INFO_NAMED ("top", "Applying (%.2f, %.2f, %.2f) for %.2f seconds", base_frame_twist_.linear.x,
194  base_frame_twist_.linear.y, base_frame_twist_.angular.z, d);
195 
196  // We'll now apply this twist open-loop for d seconds (scaled so we can guarantee stopping at the end)
197  for (double t=0; t<d; t+=1/controller_frequency_) {
199  r.sleep();
200  }
201 }
202 
203 
204 } // namespace twist_recovery
205 
d
geometry_msgs::Twist scaleGivenAccelerationLimits(const geometry_msgs::Twist &twist, const double time_remaining) const
void runBehavior()
Run the behavior.
#define ROS_INFO_NAMED(name,...)
double normalizedPoseCost(const geometry_msgs::Pose2D &pose) const
Return the cost of a pose, modified so that -1 does not equal infinity; instead 1e9 does...
#define ROS_DEBUG_STREAM_NAMED(name, args)
void publish(const boost::shared_ptr< M > &message) const
bool getRobotPose(tf::Stamped< tf::Pose > &global_pose) const
static double getYaw(const Quaternion &bt_q)
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
#define ROS_INFO_STREAM_NAMED(name, args)
gm::Pose2D forwardSimulate(const gm::Pose2D &p, const gm::Twist &twist, const double t=1.0)
tf::TransformListener * tf_
base_local_planner::CostmapModel * world_model_
geometry_msgs::Twist base_frame_twist_
costmap_2d::Costmap2DROS * global_costmap_
double angularSpeed(const gm::Twist &twist)
double linearSpeed(const gm::Twist &twist)
#define ROS_FATAL_STREAM(args)
double nonincreasingCostInterval(const geometry_msgs::Pose2D &current, const geometry_msgs::Twist &twist) const
gm::Twist scaleTwist(const gm::Twist &twist, const double scale)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool sleep()
tf::TransformListener * tf_
costmap_2d::Costmap2DROS * local_costmap_
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
ROSCPP_DECL void shutdown()
geometry_msgs::Pose2D getCurrentLocalPose() const
Costmap2D * getCostmap()
void initialize(std::string n, tf::TransformListener *tf, costmap_2d::Costmap2DROS *global_costmap, costmap_2d::Costmap2DROS *local_costmap)
Initialize the parameters of the behavior.
#define ROS_ASSERT(cond)
std::vector< geometry_msgs::Point > getRobotFootprint()


twist_recovery
Author(s): Bhaskara Marthi
autogenerated on Mon Jun 22 2020 03:18:22