42 #define GOAL_ATTRIBUTE_UNUSED 44 #define GOAL_ATTRIBUTE_UNUSED __attribute__ ((unused)) 50 return hypot(goal_x - global_pose.pose.position.x, goal_y - global_pose.pose.position.y);
54 double yaw =
tf2::getYaw(global_pose.pose.orientation);
64 nav_msgs::Path gui_path;
65 gui_path.poses.resize(path.size());
66 gui_path.header.frame_id = path[0].header.frame_id;
67 gui_path.header.stamp = path[0].header.stamp;
70 for(
unsigned int i=0; i < path.size(); i++){
71 gui_path.poses[i] = path[i];
77 void prunePlan(
const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
79 std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
80 std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
81 while(it != plan.end()){
82 const geometry_msgs::PoseStamped& w = *it;
84 double x_diff = global_pose.pose.position.x - w.pose.position.x;
85 double y_diff = global_pose.pose.position.y - w.pose.position.y;
86 double distance_sq = x_diff * x_diff + y_diff * y_diff;
88 ROS_DEBUG(
"Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.pose.position.x, global_pose.pose.position.y, w.pose.position.x, w.pose.position.y);
92 global_it = global_plan.erase(global_it);
98 const std::vector<geometry_msgs::PoseStamped>& global_plan,
99 const geometry_msgs::PoseStamped& global_pose,
101 const std::string& global_frame,
102 std::vector<geometry_msgs::PoseStamped>& transformed_plan){
103 transformed_plan.clear();
105 if (global_plan.empty()) {
106 ROS_ERROR(
"Received plan with zero length");
110 const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
114 plan_pose.header.frame_id, plan_pose.header.stamp, plan_pose.header.frame_id,
ros::Duration(0.5));
117 geometry_msgs::PoseStamped robot_pose;
118 tf.
transform(global_pose, robot_pose, plan_pose.header.frame_id);
125 double sq_dist_threshold = dist_threshold * dist_threshold;
129 while(i < (
unsigned int)global_plan.size()) {
130 double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
131 double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
132 sq_dist = x_diff * x_diff + y_diff * y_diff;
133 if (sq_dist <= sq_dist_threshold) {
139 geometry_msgs::PoseStamped newer_pose;
142 while(i < (
unsigned int)global_plan.size() && sq_dist <= sq_dist_threshold) {
143 const geometry_msgs::PoseStamped& pose = global_plan[i];
146 transformed_plan.push_back(newer_pose);
148 double x_diff = robot_pose.pose.position.x - global_plan[i].pose.position.x;
149 double y_diff = robot_pose.pose.position.y - global_plan[i].pose.position.y;
150 sq_dist = x_diff * x_diff + y_diff * y_diff;
156 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
160 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
164 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
165 if (!global_plan.empty())
166 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());
175 const std::vector<geometry_msgs::PoseStamped>& global_plan,
176 const std::string& global_frame, geometry_msgs::PoseStamped &goal_pose) {
177 if (global_plan.empty())
179 ROS_ERROR(
"Received plan with zero length");
183 const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
186 plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
192 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
196 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
200 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
201 if (global_plan.size() > 0)
202 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());
210 const std::vector<geometry_msgs::PoseStamped>& global_plan,
212 const std::string& global_frame,
213 geometry_msgs::PoseStamped& global_pose,
214 const nav_msgs::Odometry& base_odom,
215 double rot_stopped_vel,
double trans_stopped_vel,
216 double xy_goal_tolerance,
double yaw_goal_tolerance){
219 geometry_msgs::PoseStamped goal_pose;
220 getGoalPose(tf, global_plan, global_frame, goal_pose);
222 double goal_x = goal_pose.pose.position.x;
223 double goal_y = goal_pose.pose.position.y;
224 double goal_th =
tf2::getYaw(goal_pose.pose.orientation);
231 if(
stopped(base_odom, rot_stopped_vel, trans_stopped_vel))
239 bool stopped(
const nav_msgs::Odometry& base_odom,
240 const double& rot_stopped_velocity,
const double& trans_stopped_velocity){
241 return fabs(base_odom.twist.twist.angular.z) <= rot_stopped_velocity
242 && fabs(base_odom.twist.twist.linear.x) <= trans_stopped_velocity
243 && fabs(base_odom.twist.twist.linear.y) <= trans_stopped_velocity;
bool transformGlobalPlan(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const geometry_msgs::PoseStamped &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...
bool getGoalPose(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const std::string &global_frame, geometry_msgs::PoseStamped &goal_pose)
Returns last pose in plan.
unsigned int getSizeInCellsX() const
T & transform(const T &in, T &out, const std::string &target_frame, ros::Duration timeout=ros::Duration(0.0)) const
unsigned int getSizeInCellsY() const
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void doTransform(const T &data_in, T &data_out, const geometry_msgs::TransformStamped &transform)
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 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.
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 ...
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout) const
void publish(const boost::shared_ptr< M > &message) const
#define GOAL_ATTRIBUTE_UNUSED
double getYaw(const A &a)
bool isGoalReached(const tf2_ros::Buffer &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, const costmap_2d::Costmap2D &costmap, const std::string &global_frame, geometry_msgs::PoseStamped &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 publishPlan(const std::vector< geometry_msgs::PoseStamped > &path, const ros::Publisher &pub)
Publish a plan for visualization purposes.
double getResolution() const
void prunePlan(const geometry_msgs::PoseStamped &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.
def shortest_angular_distance(from_angle, to_angle)