rotate_to_goal.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Locus Robotics
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
37 #include <nav_core2/exceptions.h>
39 #include <angles/angles.h>
40 #include <string>
41 #include <vector>
42 
43 const double EPSILON = 1E-5;
44 
46 
47 namespace dwb_critics
48 {
49 
50 inline double hypot_sq(double dx, double dy)
51 {
52  return dx * dx + dy * dy;
53 }
54 
55 void RotateToGoalCritic::onInit()
56 {
57  xy_goal_tolerance_ = nav_2d_utils::searchAndGetParam(critic_nh_, "xy_goal_tolerance", 0.25);
58  xy_goal_tolerance_sq_ = xy_goal_tolerance_ * xy_goal_tolerance_;
59  double stopped_xy_velocity = nav_2d_utils::searchAndGetParam(critic_nh_, "trans_stopped_velocity", 0.25);
60  stopped_xy_velocity_sq_ = stopped_xy_velocity * stopped_xy_velocity;
61  critic_nh_.param("slowing_factor", slowing_factor_, 5.0);
62  critic_nh_.param("lookahead_time", lookahead_time_, -1.0);
63  reset();
64 }
65 
66 void RotateToGoalCritic::reset()
67 {
68  in_window_ = false;
69  rotating_ = false;
70 }
71 
72 bool RotateToGoalCritic::prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel,
73  const geometry_msgs::Pose2D& goal,
74  const nav_2d_msgs::Path2D& global_plan)
75 {
76  double dxy_sq = hypot_sq(pose.x - goal.x, pose.y - goal.y);
77  in_window_ = in_window_ || dxy_sq <= xy_goal_tolerance_sq_;
78  current_xy_speed_sq_ = hypot_sq(vel.x, vel.y);
79  rotating_ = rotating_ || (in_window_ && current_xy_speed_sq_ <= stopped_xy_velocity_sq_);
80  goal_yaw_ = goal.theta;
81  return true;
82 }
83 
84 double RotateToGoalCritic::scoreTrajectory(const dwb_msgs::Trajectory2D& traj)
85 {
86  // If we're not sufficiently close to the goal, we don't care what the twist is
87  if (!in_window_)
88  {
89  return 0.0;
90  }
91  else if (!rotating_)
92  {
93  double speed_sq = hypot_sq(traj.velocity.x, traj.velocity.y);
94  if (speed_sq >= current_xy_speed_sq_)
95  {
96  throw nav_core2::IllegalTrajectoryException(name_, "Not slowing down near goal.");
97  }
98  return speed_sq * slowing_factor_ + scoreRotation(traj);
99  }
100 
101  // If we're sufficiently close to the goal, any transforming velocity is invalid
102  if (fabs(traj.velocity.x) > EPSILON || fabs(traj.velocity.y) > EPSILON)
103  {
104  throw nav_core2::IllegalTrajectoryException(name_, "Nonrotation command near goal.");
105  }
106 
107  return scoreRotation(traj);
108 }
109 
110 double RotateToGoalCritic::scoreRotation(const dwb_msgs::Trajectory2D& traj)
111 {
112  if (traj.poses.empty())
113  {
114  throw nav_core2::IllegalTrajectoryException(name_, "Empty trajectory.");
115  }
116 
117  double end_yaw;
118  if (lookahead_time_ >= 0.0)
119  {
120  geometry_msgs::Pose2D eval_pose = dwb_local_planner::projectPose(traj, lookahead_time_);
121  end_yaw = eval_pose.theta;
122  }
123  else
124  {
125  end_yaw = traj.poses.back().theta;
126  }
127  return fabs(angles::shortest_angular_distance(end_yaw, goal_yaw_));
128 }
129 
130 } /* namespace dwb_critics */
const double EPSILON
geometry_msgs::Pose2D projectPose(const dwb_msgs::Trajectory2D &trajectory, const double time_offset)
param_t searchAndGetParam(const ros::NodeHandle &nh, const std::string &param_name, const param_t &default_value)
double hypot_sq(double dx, double dy)
Forces the commanded trajectories to only be rotations if within a certain distance window...
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
def shortest_angular_distance(from_angle, to_angle)


dwb_critics
Author(s): David V. Lu!!
autogenerated on Sun Jan 10 2021 04:08:44