15 #include <nav_msgs/Odometry.h> 40 const geometry_msgs::PoseStamped& global_pose) {
44 geometry_msgs::PoseStamped goal_pose;
45 if ( ! planner_util->
getGoal(goal_pose)) {
49 double goal_x = goal_pose.pose.position.x;
50 double goal_y = goal_pose.pose.position.y;
68 const geometry_msgs::PoseStamped& global_pose) {
74 nav_msgs::Odometry base_odom;
78 geometry_msgs::PoseStamped goal_pose;
79 if ( ! planner_util->
getGoal(goal_pose)) {
83 double goal_x = goal_pose.pose.position.x;
84 double goal_y = goal_pose.pose.position.y;
94 ROS_DEBUG(
"Goal position reached (check), stopping and turning in place");
97 double goal_th =
tf2::getYaw(goal_pose.pose.orientation);
111 const geometry_msgs::PoseStamped& robot_vel,
112 geometry_msgs::Twist& cmd_vel,
113 Eigen::Vector3f acc_lim,
115 boost::function<
bool (Eigen::Vector3f pos,
117 Eigen::Vector3f vel_samples)> obstacle_check) {
121 double vx =
sign(robot_vel.pose.position.x) * std::max(0.0, (fabs(robot_vel.pose.position.x) - acc_lim[0] * sim_period));
122 double vy =
sign(robot_vel.pose.position.y) * std::max(0.0, (fabs(robot_vel.pose.position.y) - acc_lim[1] * sim_period));
124 double vel_yaw =
tf2::getYaw(robot_vel.pose.orientation);
125 double vth =
sign(vel_yaw) * std::max(0.0, (fabs(vel_yaw) - acc_lim[2] * sim_period));
128 double yaw =
tf2::getYaw(global_pose.pose.orientation);
129 bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
130 Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
131 Eigen::Vector3f(vx, vy, vth));
135 ROS_DEBUG_NAMED(
"latched_stop_rotate",
"Slowing down... using vx, vy, vth: %.2f, %.2f, %.2f", vx, vy, vth);
136 cmd_vel.linear.x = vx;
137 cmd_vel.linear.y = vy;
138 cmd_vel.angular.z = vth;
141 ROS_WARN(
"Stopping cmd in collision");
142 cmd_vel.linear.x = 0.0;
143 cmd_vel.linear.y = 0.0;
144 cmd_vel.angular.z = 0.0;
149 const geometry_msgs::PoseStamped& global_pose,
150 const geometry_msgs::PoseStamped& robot_vel,
152 geometry_msgs::Twist& cmd_vel,
153 Eigen::Vector3f acc_lim,
156 boost::function<
bool (Eigen::Vector3f pos,
158 Eigen::Vector3f vel_samples)> obstacle_check) {
159 double yaw =
tf2::getYaw(global_pose.pose.orientation);
160 double vel_yaw =
tf2::getYaw(robot_vel.pose.orientation);
161 cmd_vel.linear.x = 0;
162 cmd_vel.linear.y = 0;
168 double max_acc_vel = fabs(vel_yaw) + acc_lim[2] * sim_period;
169 double min_acc_vel = fabs(vel_yaw) - acc_lim[2] * sim_period;
171 v_theta_samp = std::min(std::max(fabs(v_theta_samp), min_acc_vel), max_acc_vel);
174 double max_speed_to_stop =
sqrt(2 * acc_lim[2] * fabs(ang_diff));
175 v_theta_samp = std::min(max_speed_to_stop, fabs(v_theta_samp));
180 v_theta_samp = - v_theta_samp;
184 bool valid_cmd = obstacle_check(Eigen::Vector3f(global_pose.pose.position.x, global_pose.pose.position.y, yaw),
185 Eigen::Vector3f(robot_vel.pose.position.x, robot_vel.pose.position.y, vel_yaw),
186 Eigen::Vector3f( 0.0, 0.0, v_theta_samp));
189 ROS_DEBUG_NAMED(
"dwa_local_planner",
"Moving to desired goal orientation, th cmd: %.2f, valid_cmd: %d", v_theta_samp, valid_cmd);
190 cmd_vel.angular.z = v_theta_samp;
193 ROS_WARN(
"Rotation cmd in collision");
194 cmd_vel.angular.z = 0.0;
200 Eigen::Vector3f acc_lim,
204 const geometry_msgs::PoseStamped& global_pose,
205 boost::function<
bool (Eigen::Vector3f pos,
207 Eigen::Vector3f vel_samples)> obstacle_check) {
209 geometry_msgs::PoseStamped goal_pose;
210 if ( ! planner_util->
getGoal(goal_pose)) {
220 ROS_INFO(
"Goal position reached, stopping and turning in place");
224 double goal_th =
tf2::getYaw(goal_pose.pose.orientation);
228 cmd_vel.linear.x = 0.0;
229 cmd_vel.linear.y = 0.0;
230 cmd_vel.angular.z = 0.0;
234 geometry_msgs::PoseStamped robot_vel;
236 nav_msgs::Odometry base_odom;
237 odom_helper_.
getOdom(base_odom);
bool rotateToGoal(const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel, double goal_th, geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, base_local_planner::LocalPlannerLimits &limits, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
Once a goal position is reached... rotate to the goal orientation.
bool stopWithAccLimits(const geometry_msgs::PoseStamped &global_pose, const geometry_msgs::PoseStamped &robot_vel, geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
Stop the robot taking into account acceleration limits.
void getOdom(nav_msgs::Odometry &base_odom)
bool isPositionReached(LocalPlannerUtil *planner_util, const geometry_msgs::PoseStamped &global_pose)
double getGoalPositionDistance(const geometry_msgs::PoseStamped &global_pose, double goal_x, double goal_y)
return squared distance to check if the goal position has been achieved
bool isGoalReached(LocalPlannerUtil *planner_util, OdometryHelperRos &odom_helper, const geometry_msgs::PoseStamped &global_pose)
LatchedStopRotateController(const std::string &name="")
bool stopped(const nav_msgs::Odometry &base_odom, const double &rot_stopped_velocity, const double &trans_stopped_velocity)
Check whether the robot is stopped or not.
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
bool latch_xy_goal_tolerance_
bool computeVelocityCommandsStopRotate(geometry_msgs::Twist &cmd_vel, Eigen::Vector3f acc_lim, double sim_period, LocalPlannerUtil *planner_util, OdometryHelperRos &odom_helper, const geometry_msgs::PoseStamped &global_pose, boost::function< bool(Eigen::Vector3f pos, Eigen::Vector3f vel, Eigen::Vector3f vel_samples)> obstacle_check)
#define ROS_DEBUG_NAMED(name,...)
double getGoalOrientationAngleDifference(const geometry_msgs::PoseStamped &global_pose, double goal_th)
return angle difference to goal to check if the goal orientation has been achieved ...
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
LocalPlannerLimits getCurrentLimits()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
bool getGoal(geometry_msgs::PoseStamped &goal_pose)
double getYaw(const A &a)
void getRobotVel(geometry_msgs::PoseStamped &robot_vel)
Helper class implementing infrastructure code many local planner implementations may need...
virtual ~LatchedStopRotateController()
def shortest_angular_distance(from_angle, to_angle)
double yaw_goal_tolerance