43 void set_angle(geometry_msgs::PoseStamped* pose,
double angle)
48 double getYaw(geometry_msgs::PoseStamped pose)
54 std::vector<geometry_msgs::PoseStamped>& path)
60 for(
int i=0;i<n-1;i++){
65 for(
int i=0;i<n-1;i++){
71 for(
int i=0;i<n-1;i++){
77 for(
int i=0;i<n-1;i++){
83 path[0].pose.orientation = start.pose.orientation;
87 for(
int i=0;i<n-1;i++){
92 double last =
getYaw(path[i]);
94 double new_angle =
getYaw(path[i-1]);
102 path[0].pose.orientation = start.pose.orientation;
111 int index1 = std::min((
int)path.size() - 1, index +
window_size_);
113 double x0 = path[index0].pose.position.x,
114 y0 = path[index0].pose.position.y,
115 x1 = path[index1].pose.position.x,
116 y1 = path[index1].pose.position.y;
123 int start_index,
int end_index)
125 double start_yaw =
getYaw(path[start_index]),
126 end_yaw =
getYaw(path[end_index ]);
128 double increment = diff/(end_index-start_index);
129 for(
int i=start_index; i<=end_index; i++){
130 double angle = start_yaw + increment * i;
virtual void processPath(const geometry_msgs::PoseStamped &start, std::vector< geometry_msgs::PoseStamped > &path)
static double getYaw(const Quaternion &bt_q)
void set_angle(geometry_msgs::PoseStamped *pose, double angle)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void interpolate(std::vector< geometry_msgs::PoseStamped > &path, int start_index, int end_index)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
void setAngleBasedOnPositionDerivative(std::vector< geometry_msgs::PoseStamped > &path, int index)
def normalize_angle(angle)
double getYaw(geometry_msgs::PoseStamped pose)
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
def shortest_angular_distance(from_angle, to_angle)