22#include <angles/angles.h>
23#include <tf2/transform_datatypes.h>
24#include <geometry_msgs/msg/quaternion.hpp>
25#include <geometry_msgs/msg/quaternion_stamped.hpp>
26#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
32#include <geometry_msgs/msg/pose_stamped.hpp>
33#include <geometry_msgs/msg/twist.hpp>
38 const geometry_msgs::msg::PoseStamped & start,
double dstRads,
39 std::vector<geometry_msgs::msg::PoseStamped> & plan,
double radstep)
41 double startYaw = tf2::getYaw(start.pose.orientation);
46 double goalAngleOffset = angles::shortest_angular_distance(startYaw, dstRads);
49 if (goalAngleOffset >= 0)
53 for (
double dangle = 0; dangle <= goalAngleOffset; dangle += radstep)
55 geometry_msgs::msg::PoseStamped p = start;
56 double yaw = startYaw + dangle;
61 p.pose.orientation = tf2::toMsg(q);
69 for (
double dangle = 0; dangle >= goalAngleOffset; dangle -= radstep)
72 geometry_msgs::msg::PoseStamped p = start;
73 double yaw = startYaw + dangle;
78 p.pose.orientation = tf2::toMsg(q);
84 geometry_msgs::msg::PoseStamped end = start;
86 q.setRPY(0, 0, dstRads);
87 end.pose.orientation = tf2::toMsg(q);
94 const geometry_msgs::msg::PoseStamped & startOrientedPose,
const geometry_msgs::msg::Point & goal,
95 double length, std::vector<geometry_msgs::msg::PoseStamped> & plan)
98 double steps = length / dx;
99 double dt = 1.0 / steps;
104 plan.push_back(startOrientedPose);
108 for (
double t = 0; t <= 1.0; t += dt)
110 geometry_msgs::msg::PoseStamped p = startOrientedPose;
112 p.pose.position.x = startOrientedPose.pose.position.x * (1 - t) + goal.x * t;
113 p.pose.position.y = startOrientedPose.pose.position.y * (1 - t) + goal.y * t;
114 p.pose.orientation = startOrientedPose.pose.orientation;
124std::ostream &
operator<<(std::ostream & out,
const geometry_msgs::msg::Twist & msg)
126 out <<
"Twist [" << msg.linear.x <<
"m , " << msg.linear.y <<
"m , " << msg.angular.z <<
"rad ]";
130std::ostream &
operator<<(std::ostream & out,
const geometry_msgs::msg::Pose & msg)
132 out <<
"Position [" << msg.position.x <<
"m , " << msg.position.y <<
"m , " << msg.position.z
134 out <<
" Orientation [" << msg.orientation.x <<
" , " << msg.orientation.y <<
" , "
135 << msg.orientation.z <<
", " << msg.orientation.w <<
"]";
139std::ostream &
operator<<(std::ostream & out,
const geometry_msgs::msg::Point & msg)
141 out <<
"Position [" << msg.x <<
"m , " << msg.y <<
"m , " << msg.z <<
"m ]";
145std::ostream &
operator<<(std::ostream & out,
const geometry_msgs::msg::Quaternion & msg)
147 out <<
"Quaternion [" << msg.x <<
" , " << msg.y <<
" , " << msg.z <<
", " << msg.w <<
"]";
151std::ostream &
operator<<(std::ostream & out,
const geometry_msgs::msg::PoseStamped & msg)
std::ostream & operator<<(std::ostream &out, const geometry_msgs::msg::Quaternion &msg)
geometry_msgs::msg::PoseStamped makePureSpinningSubPlan(const geometry_msgs::msg::PoseStamped &start, double dstRads, std::vector< geometry_msgs::msg::PoseStamped > &plan, double radstep=0.005)
geometry_msgs::msg::PoseStamped makePureStraightSubPlan(const geometry_msgs::msg::PoseStamped &startOrientedPose, const geometry_msgs::msg::Point &goal, double length, std::vector< geometry_msgs::msg::PoseStamped > &plan)