pose_base_controller.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2009, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
38 
39 namespace pose_base_controller {
40  PoseBaseController::PoseBaseController() : action_server_(ros::NodeHandle(),
41  "pose_base_controller",
42  boost::bind(&PoseBaseController::execute, this, _1),
43  false){
44  ros::NodeHandle node_private("~");
45  node_private.param("k_trans", K_trans_, 1.0);
46  node_private.param("k_rot", K_rot_, 1.0);
47 
48  node_private.param("tolerance_trans", tolerance_trans_, 0.02);
49  node_private.param("tolerance_rot", tolerance_rot_, 0.04);
50  node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
51 
52  node_private.param("fixed_frame", fixed_frame_, std::string("odom_combined"));
53  node_private.param("base_frame", base_frame_, std::string("base_link"));
54 
55  node_private.param("holonomic", holonomic_, true);
56 
57  node_private.param("max_vel_lin", max_vel_lin_, 0.9);
58  node_private.param("max_vel_th", max_vel_th_, 1.4);
59 
60  node_private.param("min_vel_lin", min_vel_lin_, 0.0);
61  node_private.param("min_vel_th", min_vel_th_, 0.0);
62  node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
63  node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
64  node_private.param("frequency", freq_, 100.0);
65  node_private.param("transform_tolerance", transform_tolerance_, 0.5);
66 
67  node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
68  node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
69 
70  ros::NodeHandle node;
71  odom_sub_ = node.subscribe<nav_msgs::Odometry>("odom", 1, boost::bind(&PoseBaseController::odomCallback, this, _1));
72  vel_pub_ = node.advertise<geometry_msgs::Twist>("base_controller/command", 10);
73 
75  ROS_DEBUG("Started server");
76  }
77 
78  void PoseBaseController::odomCallback(const nav_msgs::Odometry::ConstPtr& msg){
79  //we assume that the odometry is published in the frame of the base
80  boost::mutex::scoped_lock lock(odom_lock_);
81  base_odom_.twist.twist.linear.x = msg->twist.twist.linear.x;
82  base_odom_.twist.twist.linear.y = msg->twist.twist.linear.y;
83  base_odom_.twist.twist.angular.z = msg->twist.twist.angular.z;
84  ROS_DEBUG("In the odometry callback with velocity values: (%.2f, %.2f, %.2f)",
85  base_odom_.twist.twist.linear.x, base_odom_.twist.twist.linear.y, base_odom_.twist.twist.angular.z);
86  }
87 
88  double PoseBaseController::headingDiff(double x, double y, double pt_x, double pt_y, double heading)
89  {
90  double v1_x = x - pt_x;
91  double v1_y = y - pt_y;
92  double v2_x = cos(heading);
93  double v2_y = sin(heading);
94 
95  double perp_dot = v1_x * v2_y - v1_y * v2_x;
96  double dot = v1_x * v2_x + v1_y * v2_y;
97 
98  //get the signed angle
99  double vector_angle = atan2(perp_dot, dot);
100 
101  return -1.0 * vector_angle;
102  }
103 
105  tf::Stamped<tf::Pose> global_pose, robot_pose;
106  global_pose.setIdentity();
107  robot_pose.setIdentity();
108  robot_pose.frame_id_ = base_frame_;
109  robot_pose.stamp_ = ros::Time();
110 
111  tf_.transformPose(fixed_frame_, robot_pose, global_pose);
112  //ROS_INFO("Delay: %f", (global_pose.stamp_ - ros::Time::now()).toSec());
113  return global_pose;
114  }
115 
116  move_base_msgs::MoveBaseGoal PoseBaseController::goalToFixedFrame(const move_base_msgs::MoveBaseGoal& goal){
117  tf::Stamped<tf::Pose> pose, fixed_pose;
118  tf::poseStampedMsgToTF(goal.target_pose, pose);
119 
120  //just get the latest available transform... for accuracy they should send
121  //goals in the frame of the planner
122  pose.stamp_ = ros::Time();
123 
124  try{
125  tf_.transformPose(fixed_frame_, pose, fixed_pose);
126  }
127  catch(tf::TransformException& ex){
128  ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
129  pose.frame_id_.c_str(), fixed_frame_.c_str(), ex.what());
130  return goal;
131  }
132 
133  move_base_msgs::MoveBaseGoal fixed_goal;
134  tf::poseStampedTFToMsg(fixed_pose, fixed_goal.target_pose);
135  return fixed_goal;
136 
137  }
138 
139  void PoseBaseController::execute(const move_base_msgs::MoveBaseGoalConstPtr& user_goal){
140  bool success = false;
141 
142  move_base_msgs::MoveBaseGoal goal = goalToFixedFrame(*user_goal);
143 
144  success = controlLoop(goal);
145 
146  //based on the control loop's exit status... we'll set our goal status
147  if(success){
148  //wait until we're stopped before returning success
149  ros::Rate r(10.0);
150  while(!stopped()){
151  geometry_msgs::Twist empty_twist;
152  vel_pub_.publish(empty_twist);
153  r.sleep();
154  }
156  }
157  else{
158  //if a preempt was requested... the control loop exits for that reason
161  else
163  }
164  }
165 
167  //copy over the odometry information
168  nav_msgs::Odometry base_odom;
169  {
170  boost::mutex::scoped_lock lock(odom_lock_);
171  base_odom = base_odom_;
172  }
173 
174  return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity_
175  && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity_
176  && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity_;
177  }
178 
179  bool PoseBaseController::controlLoop(const move_base_msgs::MoveBaseGoal& current_goal){
180  if(freq_ == 0.0)
181  return false;
182 
183  ros::Rate r(freq_);
184  ros::Time goal_reached_time = ros::Time::now();
185  while(ros::ok()){
187  return false;
188  }
189  ROS_DEBUG("At least looping");
190  tf::Stamped<tf::Pose> target_pose;
191  tf::poseStampedMsgToTF(current_goal.target_pose, target_pose);
192 
193 
194  //get the current pose of the robot in the fixed frame
195  tf::Stamped<tf::Pose> robot_pose;
196  try {
197  robot_pose = getRobotPose();
198  //make sure that the transform to the dance frame isn't too out of date
200  ROS_WARN("The %s frame to %s frame transform is more than %.2f seconds old, will not move",
201  fixed_frame_.c_str(), base_frame_.c_str(), transform_tolerance_);
202  geometry_msgs::Twist empty_twist;
203  vel_pub_.publish(empty_twist);
204  return false;
205  }
206  }
207  catch(tf::TransformException &ex){
208  ROS_ERROR("Can't transform: %s\n", ex.what());
209  geometry_msgs::Twist empty_twist;
210  vel_pub_.publish(empty_twist);
211  return false;
212  }
213  ROS_DEBUG("PoseBaseController: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf::getYaw(robot_pose.getRotation()));
214  ROS_DEBUG("PoseBaseController: target robot pose %f %f ==> %f", target_pose.getOrigin().x(), target_pose.getOrigin().y(), tf::getYaw(target_pose.getRotation()));
215 
216  //get the difference between the two poses
217  geometry_msgs::Twist diff = diff2D(target_pose, robot_pose);
218  ROS_DEBUG("PoseBaseController: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
219 
220  //publish the desired velocity command to the base
221  vel_pub_.publish(limitTwist(diff));
222 
223  //if we haven't reached our goal... we'll update time
224  if (fabs(diff.linear.x) > tolerance_trans_ || fabs(diff.linear.y) > tolerance_trans_ || fabs(diff.angular.z) > tolerance_rot_)
225  goal_reached_time = ros::Time::now();
226 
227  //check if we've reached our goal for long enough to succeed
228  if(goal_reached_time + ros::Duration(tolerance_timeout_) < ros::Time::now()){
229  geometry_msgs::Twist empty_twist;
230  vel_pub_.publish(empty_twist);
231  return true;
232  }
233 
234  r.sleep();
235  }
236  return false;
237  }
238 
239  geometry_msgs::Twist PoseBaseController::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
240  {
241  geometry_msgs::Twist res;
242  tf::Pose diff = pose2.inverse() * pose1;
243  res.linear.x = diff.getOrigin().x();
244  res.linear.y = diff.getOrigin().y();
245  res.angular.z = tf::getYaw(diff.getRotation());
246 
247  if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
248  return res;
249 
250  //in the case that we're not rotating to our goal position and we have a non-holonomic robot
251  //we'll need to command a rotational velocity that will help us reach our desired heading
252 
253  //we want to compute a goal based on the heading difference between our pose and the target
254  double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
255  pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));
256 
257  //we'll also check if we can move more effectively backwards
258  double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
259  pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));
260 
261  //check if its faster to just back up
262  if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
263  ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
264  yaw_diff = neg_yaw_diff;
265  }
266 
267  //compute the desired quaterion
268  tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
269  tf::Quaternion rot = pose2.getRotation() * rot_diff;
270  tf::Pose new_pose = pose1;
271  new_pose.setRotation(rot);
272 
273  diff = pose2.inverse() * new_pose;
274  res.linear.x = diff.getOrigin().x();
275  res.linear.y = diff.getOrigin().y();
276  res.angular.z = tf::getYaw(diff.getRotation());
277  return res;
278  }
279 
280 
281  geometry_msgs::Twist PoseBaseController::limitTwist(const geometry_msgs::Twist& twist)
282  {
283  geometry_msgs::Twist res = twist;
284  res.linear.x *= K_trans_;
285  if(!holonomic_)
286  res.linear.y = 0.0;
287  else
288  res.linear.y *= K_trans_;
289  res.angular.z *= K_rot_;
290 
291  //make sure to bound things by our velocity limits
292  double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
293  if (lin_overshoot > 1.0)
294  {
295  res.linear.x /= lin_overshoot;
296  res.linear.y /= lin_overshoot;
297  }
298  if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
299  if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
300 
301  if(fabs(res.linear.x) < in_place_trans_vel_ && fabs(res.linear.y) < in_place_trans_vel_){
302  if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
303  }
304 
305  ROS_DEBUG("Angular command %f", res.angular.z);
306  return res;
307  }
308 };
309 
310 int main(int argc, char** argv)
311 {
312  ros::init(argc, argv, "pose_base_controller");
313 
315 
316  ros::spin();
317  return 0;
318 }
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static void poseStampedTFToMsg(const Stamped< Pose > &bt, geometry_msgs::PoseStamped &msg)
static double getYaw(const Quaternion &bt_q)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define ROS_WARN(...)
geometry_msgs::Twist limitTwist(const geometry_msgs::Twist &twist)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
geometry_msgs::Twist diff2D(const tf::Pose &pose1, const tf::Pose &pose2)
ROSCPP_DECL void spin(Spinner &spinner)
void odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
void execute(const move_base_msgs::MoveBaseGoalConstPtr &user_goal)
TFSIMD_FORCE_INLINE const tfScalar & x() const
void setPreempted(const Result &result=Result(), const std::string &text=std::string(""))
static Quaternion createQuaternionFromYaw(double yaw)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
bool controlLoop(const move_base_msgs::MoveBaseGoal &current_goal)
ROSCPP_DECL bool execute(const std::string &method, const XmlRpc::XmlRpcValue &request, XmlRpc::XmlRpcValue &response, XmlRpc::XmlRpcValue &payload, bool wait_for_master)
Transform inverse() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
TFSIMD_FORCE_INLINE tfScalar dot(const Quaternion &q1, const Quaternion &q2)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
bool sleep()
int main(int argc, char **argv)
ros::Time stamp_
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
std::string frame_id_
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Quaternion getRotation() const
static Time now()
static void poseStampedMsgToTF(const geometry_msgs::PoseStamped &msg, Stamped< Pose > &bt)
move_base_msgs::MoveBaseGoal goalToFixedFrame(const move_base_msgs::MoveBaseGoal &goal)
#define ROS_ERROR(...)
double headingDiff(double pt_x, double pt_y, double x, double y, double heading)
#define ROS_DEBUG(...)


pose_base_controller
Author(s): Eitan Marder-Eppstein
autogenerated on Mon Jun 22 2020 03:18:13