move in semi-circle

src/TimedPlanner.cpp

@@ -10,6 +10,7 @@
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 #include <trajectory_msgs/JointTrajectoryPoint.h>
 #include <tf2/transform_datatypes.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
 #include "util/TrajectoryUtil.h"
 
@@ -29,26 +30,22 @@ namespace PlannerState {
 
 }
 
-void initCircularTrajector(moveit::planning_interface::MoveGroupInterface *group, double radius, double origin_x, double origin_y, double origin_z, double granularity){
+void initCircularTrajectory(double radius, geometry_msgs::Point origin, double granularity){
 
-    geometry_msgs::Pose startPose = group->getCurrentPose().pose;
     double step_size_rad = 2 * M_PI / granularity;
 
     for(double i = 0; i < granularity; i++){
 
-        double next_rad = step_size_rad * i;
+        double next_rad = step_size_rad * (2*i > granularity ? granularity - i : i);
 
-        tf2::Quaternion orientation{0, M_PI, next_rad};
+        tf2::Quaternion orientation; // the RPY constructor is deprecated
+        orientation.setRPY(0, M_PI, next_rad);
 
         geometry_msgs::Pose nextPose;
-        nextPose.orientation.w = orientation.w();
-        nextPose.orientation.x = orientation.x();
-        nextPose.orientation.y = orientation.y();
-        nextPose.orientation.z = orientation.z();
-
-        nextPose.position.z = origin_z;
-        nextPose.position.y = origin_y + radius * cos(next_rad);
-        nextPose.position.x = origin_x + radius * sin(next_rad);
+        nextPose.orientation = tf2::toMsg(orientation);
+        nextPose.position.x = origin.x + radius * sin(next_rad);
+        nextPose.position.y = origin.y + radius * cos(next_rad);
+        nextPose.position.z = origin.z;
 
         PlannerState::raw_trajectory.push_back(nextPose);
         std::cout << "added: " << nextPose << std::endl;
@@ -60,7 +57,9 @@ void initRawTrajectory(ros::NodeHandle node_handle, moveit::planning_interface::
     geometry_msgs::Pose current_pose = group->getCurrentPose().pose;
 
     if(useCircularDefault){
-        initCircularTrajector(group, 0.6, 0.0, 0.0, 0.3, 20.0);
+        geometry_msgs::Point origin;
+        origin.z = .3;
+        initCircularTrajectory(0.6, origin, 20.0);
         return;
     }