42 void PoseToPose2D(
const geometry_msgs::Pose pose, geometry_msgs::Pose2D& pose2D)
51 double useless_pitch, useless_roll, yaw;
58 pose2D.x = pose.position.x;
59 pose2D.y = pose.position.y;
66 void Pose2DToPose(geometry_msgs::Pose& pose,
const geometry_msgs::Pose2D pose2D)
75 pose.position.x = pose2D.x;
76 pose.position.y = pose2D.y;
77 pose.position.z = 0.0;
80 pose.orientation.x = frame_quat.x();
81 pose.orientation.y = frame_quat.y();
82 pose.orientation.z = frame_quat.z();
83 pose.orientation.w = frame_quat.w();
91 std::vector<geometry_msgs::PoseStamped>& transformed_plan, std::vector<int>& start_end_counts)
93 const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
96 transformed_plan.clear();
100 if (!global_plan.size() > 0)
102 ROS_ERROR(
"Recieved plan with zero length");
108 plan_pose.header.frame_id, transform);
112 robot_pose.setIdentity();
115 tf.
transformPose(plan_pose.header.frame_id, robot_pose, robot_pose);
119 double dist_threshold = std::max(
125 double sq_dist_threshold = dist_threshold * dist_threshold;
130 std::vector<int> start_end_count;
131 start_end_count.assign(2, 0);
135 ROS_ASSERT( (start_end_counts.at(0) > 0) && (start_end_counts.at(0) <= (int) global_plan.size()) );
136 i = (
unsigned int) global_plan.size() - (
unsigned int) start_end_counts.at(0);
140 while(i < (
unsigned int)global_plan.size() && sq_dist > sq_dist_threshold)
142 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
143 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
144 sq_dist = x_diff * x_diff + y_diff * y_diff;
148 if( sq_dist > sq_dist_threshold )
152 start_end_count.at(0) = (int) (((
unsigned int) global_plan.size()) - i);
158 geometry_msgs::PoseStamped newer_pose;
161 while(i < (
unsigned int)global_plan.size() && sq_dist < sq_dist_threshold)
163 double x_diff = robot_pose.getOrigin().x() - global_plan[i].pose.position.x;
164 double y_diff = robot_pose.getOrigin().y() - global_plan[i].pose.position.y;
165 sq_dist = x_diff * x_diff + y_diff * y_diff;
167 const geometry_msgs::PoseStamped& pose = global_plan[i];
168 poseStampedMsgToTF(pose, tf_pose);
169 tf_pose.
setData(transform * tf_pose);
170 tf_pose.stamp_ = transform.
stamp_;
171 tf_pose.frame_id_ = global_frame;
172 poseStampedTFToMsg(tf_pose, newer_pose);
174 transformed_plan.push_back(newer_pose);
178 start_end_count.at(1) = int (((
unsigned int) global_plan.size()) - i);
186 start_end_counts = start_end_count;
191 ROS_ERROR(
"No Transform available Error: %s\n", ex.what());
196 ROS_ERROR(
"Connectivity Error: %s\n", ex.what());
201 ROS_ERROR(
"Extrapolation Error: %s\n", ex.what());
202 if (global_plan.size() > 0)
203 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());
213 double max_distance_sqr = 0;
214 for (
size_t i = 0; i < footprint.size(); ++i) {
215 geometry_msgs::Point& p = footprint[i];
216 double distance_sqr = p.x*p.x + p.y*p.y;
217 if (distance_sqr > max_distance_sqr) {
218 max_distance_sqr = distance_sqr;
221 return sqrt(max_distance_sqr);
static void poseMsgToTF(const geometry_msgs::Pose &msg, Pose &bt)
void setData(const T &input)
void getEulerYPR(tfScalar &yaw, tfScalar &pitch, tfScalar &roll, unsigned int solution_number=1) const
double getSizeInMetersX() const
void Pose2DToPose(geometry_msgs::Pose &pose, const geometry_msgs::Pose2D pose2D)
Converts a frame of type Pose to type Pose2D (mainly -> conversion of orientation from euler angles t...
double getSizeInMetersY() const
static Quaternion createQuaternionFromYaw(double yaw)
double getCircumscribedRadius(costmap_2d::Costmap2DROS &costmap)
Gets the footprint of the robot and computes the circumscribed radius for the eband approach...
std::string getBaseFrameID()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
def normalize_angle(angle)
std::vector< geometry_msgs::Point > getRobotFootprint()
bool transformGlobalPlan(const tf::TransformListener &tf, const std::vector< geometry_msgs::PoseStamped > &global_plan, costmap_2d::Costmap2DROS &costmap, const std::string &global_frame, std::vector< geometry_msgs::PoseStamped > &transformed_plan, std::vector< int > &start_end_counts_from_end)
Transforms the global plan of the robot from the planner frame to the local frame. This replaces the transformGlobalPlan as defined in the base_local_planner/goal_functions.h main difference is that it additionally outputs counter indicating which part of the plan has been transformed.
void PoseToPose2D(const geometry_msgs::Pose pose, geometry_msgs::Pose2D &pose2D)
Converts a frame of type Pose to type Pose2D (mainly -> conversion of orientation from quaternions to...