42 #include <geometry_msgs/Twist.h>
43 #include <geometry_msgs/Point.h>
63 local_costmap_ = local_costmap;
70 private_nh.param(
"sim_granularity", sim_granularity_, 0.017);
71 private_nh.param(
"frequency", frequency_, 20.0);
76 blp_nh.param(
"yaw_goal_tolerance", tolerance_, 0.10);
84 ROS_ERROR(
"You should not call initialize twice on this object, doing nothing");
88 RotateRecovery::~RotateRecovery()
93 void RotateRecovery::runBehavior()
97 ROS_ERROR(
"This object must be initialized before runBehavior is called");
101 if (local_costmap_ == NULL)
103 ROS_ERROR(
"The costmap passed to the RotateRecovery object cannot be NULL. Doing nothing.");
106 ROS_WARN(
"Rotate recovery behavior started.");
112 geometry_msgs::PoseStamped global_pose;
113 local_costmap_->getRobotPose(global_pose);
115 double current_angle =
tf2::getYaw(global_pose.pose.orientation);
116 double start_angle = current_angle;
118 bool got_180 =
false;
125 local_costmap_->getRobotPose(global_pose);
126 current_angle =
tf2::getYaw(global_pose.pose.orientation);
134 dist_left = M_PI + distance_to_180;
136 if (distance_to_180 < tolerance_)
147 double x = global_pose.pose.position.x, y = global_pose.pose.position.y;
150 double sim_angle = 0.0;
151 while (sim_angle < dist_left)
153 double theta = current_angle + sim_angle;
156 double footprint_cost = world_model_->footprintCost(x, y, theta, local_costmap_->getRobotFootprint(), 0.0, 0.0);
157 if (footprint_cost < 0.0)
159 ROS_ERROR(
"Rotate recovery can't rotate in place because there is a potential collision. Cost: %.2f",
164 sim_angle += sim_granularity_;
168 double vel = sqrt(2 * acc_lim_th_ * dist_left);
171 vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
173 geometry_msgs::Twist cmd_vel;
174 cmd_vel.linear.x = 0.0;
175 cmd_vel.linear.y = 0.0;
176 cmd_vel.angular.z = vel;