42 return hypot(goal_x - global_pose.getOrigin().x(), goal_y - global_pose.getOrigin().y());
46 double yaw =
tf::getYaw(global_pose.getRotation());
56 nav_msgs::Path gui_path;
57 gui_path.poses.resize(path.size());
58 gui_path.header.frame_id = path[0].header.frame_id;
59 gui_path.header.stamp = path[0].header.stamp;
62 for(
unsigned int i=0; i < path.size(); i++){
63 gui_path.poses[i] = path[i];
69 void prunePlan(
const tf::Stamped<tf::Pose>& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
71 std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
72 std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
73 while(it != plan.end()){
74 const geometry_msgs::PoseStamped&
w = *it;
76 double x_diff = global_pose.getOrigin().x() - w.pose.position.x;
77 double y_diff = global_pose.getOrigin().y() - w.pose.position.y;
78 double distance_sq = x_diff * x_diff + y_diff * y_diff;
80 ROS_DEBUG(
"Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.getOrigin().x(), global_pose.getOrigin().y(), w.pose.position.x, w.pose.position.y);
84 global_it = global_plan.erase(global_it);
90 const std::vector<geometry_msgs::PoseStamped>& global_plan,
93 const std::string& global_frame,
94 std::vector<geometry_msgs::PoseStamped>& transformed_plan){
95 transformed_plan.clear();
97 if (global_plan.empty()) {
98 ROS_ERROR(
"Received plan with zero length");
102 const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
107 plan_pose.header.frame_id, plan_pose.header.stamp,
110 plan_pose.header.frame_id, plan_pose.header.stamp,
111 plan_pose.header.frame_id, plan_to_global_transform);
115 tf.
transformPose(plan_pose.header.frame_id, global_pose, robot_pose);
122 double sq_dist_threshold = dist_threshold * dist_threshold;
126 while(i < (
unsigned int)global_plan.size()) {
127 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
128 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
129 sq_dist = x_diff * x_diff + y_diff * y_diff;
130 if (sq_dist <= sq_dist_threshold) {
137 geometry_msgs::PoseStamped newer_pose;
140 while(i < (
unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
141 const geometry_msgs::PoseStamped& pose = global_plan[i];
142 poseStampedMsgToTF(pose, tf_pose);
143 tf_pose.
setData(plan_to_global_transform * tf_pose);
144 tf_pose.stamp_ = plan_to_global_transform.
stamp_;
145 tf_pose.frame_id_ = global_frame;
146 poseStampedTFToMsg(tf_pose, newer_pose);
148 transformed_plan.push_back(newer_pose);
150 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
151 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
152 sq_dist = x_diff * x_diff + y_diff * y_diff;
158 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
162 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
166 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
167 if (!global_plan.empty())
168 ROS_ERROR(
"Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (
unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
177 const std::vector<geometry_msgs::PoseStamped>& global_plan,
179 if (global_plan.empty())
181 ROS_ERROR(
"Received plan with zero length");
185 const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
189 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
192 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
193 plan_goal_pose.header.frame_id, transform);
195 poseStampedMsgToTF(plan_goal_pose, goal_pose);
196 goal_pose.
setData(transform * goal_pose);
197 goal_pose.stamp_ = transform.
stamp_;
198 goal_pose.frame_id_ = global_frame;
202 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
206 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
210 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
211 if (global_plan.size() > 0)
212 ROS_ERROR(
"Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (
unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
220 const std::vector<geometry_msgs::PoseStamped>& global_plan,
222 const std::string& global_frame,
224 const nav_msgs::Odometry& base_odom,
225 double rot_stopped_vel,
double trans_stopped_vel,
226 double xy_goal_tolerance,
double yaw_goal_tolerance){
230 getGoalPose(tf, global_plan, global_frame, goal_pose);
232 double goal_x = goal_pose.getOrigin().getX();
233 double goal_y = goal_pose.getOrigin().getY();
234 double goal_th =
tf::getYaw(goal_pose.getRotation());
241 if(
stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
249 bool stopped(
const nav_msgs::Odometry& base_odom,
250 const double& rot_stopped_velocity,
const double& trans_stopped_velocity){
251 return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
252 && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
253 && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
bool getGoalPose(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const std::string &global_frame, tf::Stamped< tf::Pose > &goal_pose)
Returns last pose in plan.
bool isGoalReached(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, tf::Stamped< tf::Pose > &global_pose, const nav_msgs::Odometry &base_odom, double rot_stopped_vel, double trans_stopped_vel, double xy_goal_tolerance, double yaw_goal_tolerance)
Check if the goal pose has been achieved.
void publish(const boost::shared_ptr< M > &message) const
void setData(const T &input)
double getGoalPositionDistance(const tf::Stamped< tf::Pose > &global_pose, double goal_x, double goal_y)
return squared distance to check if the goal position has been achieved
static double getYaw(const Quaternion &bt_q)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void prunePlan(const tf::Stamped< tf::Pose > &global_pose, std::vector< geometry_msgs::PoseStamped > &plan, std::vector< geometry_msgs::PoseStamped > &global_plan)
Trim off parts of the global plan that are far enough behind the robot.
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.
bool transformGlobalPlan(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const tf::Stamped< tf::Pose > &global_robot_pose, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan)
Transforms the global plan of the robot from the planner frame to the frame of the costmap...
unsigned int getSizeInCellsY() const
TFSIMD_FORCE_INLINE const tfScalar & w() const
void publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
Publish a plan for visualization purposes.
unsigned int getSizeInCellsX() const
double getResolution() const
def shortest_angular_distance(from_angle, to_angle)
double getGoalOrientationAngleDifference(const tf::Stamped< tf::Pose > &global_pose, double goal_th)
return angle difference to goal to check if the goal orientation has been achieved ...
tf::tfVector4 __attribute__