changed message formats to ros-standard complient

CMakeLists.txt

@@ -39,6 +39,7 @@ add_service_files(
 generate_messages(
         DEPENDENCIES
         std_msgs
+        geometry_msgs
 )
 
 # ################################################################################################################################

src/teaching/pose_replayer.cpp

@@ -4,6 +4,7 @@
 #include <ros/ros.h>
 
 #include <std_msgs/String.h>
+#include <geometry_msgs/Pose.h>
 
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
@@ -21,7 +22,7 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
     const robot_state::JointModelGroup *joint_model_group =
             move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
 
-    std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
+    std::vector<geometry_msgs::Pose> poses;
 
     // Visualization Setup
     namespace rvt = rviz_visual_tools;
@@ -38,32 +39,18 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
     panda_teaching::GetCollectedPoses srv;
 
     if (client.call(srv)) {
-        for (int i = 0; i < srv.response.x_transform.size(); i++) {
-            x_transform.push_back((double) srv.response.x_transform[i]);
-            y_transform.push_back((double) srv.response.y_transform[i]);
-            z_transform.push_back((double) srv.response.z_transform[i]);
-
-            x_rotation.push_back((double) srv.response.x_rotation[i]);
-            y_rotation.push_back((double) srv.response.y_rotation[i]);
-            z_rotation.push_back((double) srv.response.z_rotation[i]);
-            w_rotation.push_back((double) srv.response.w_rotation[i]);
+        for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
+            poses.push_back(srv.response.pose_array.poses[i]);
         }
     } else {
         ROS_ERROR("Failed to fetch saved poses");
         return;
     }
 
-    for (int i = 0; i < x_transform.size(); i++) {
+    for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
 
         move_group.setStartState(*move_group.getCurrentState());
-        geometry_msgs::Pose another_pose;
-        another_pose.orientation.x = x_rotation.at(i);
-        another_pose.orientation.y = y_rotation.at(i);
-        another_pose.orientation.z = z_rotation.at(i);
-        another_pose.orientation.w = w_rotation.at(i);
-        another_pose.position.x = x_transform.at(i);
-        another_pose.position.y = y_transform.at(i);
-        another_pose.position.z = z_transform.at(i);
+        geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
         move_group.setPoseTarget(another_pose);
 
         moveit::planning_interface::MoveGroupInterface::Plan my_plan;

src/teaching/pose_storage_service.cpp

@@ -5,35 +5,21 @@
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
 
-std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
+std::vector<geometry_msgs::Pose> poses;
 
 bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req,
                        panda_teaching::GetCollectedPoses::Response &res) {
 
-    res.x_transform = x_transform;
-    res.y_transform = y_transform;
-    res.z_transform = z_transform;
-
-    res.x_rotation = x_rotation;
-    res.y_rotation = y_rotation;
-    res.z_rotation = z_rotation;
-    res.w_rotation = w_rotation;
-
+    res.pose_array.poses = poses;
     return true;
 }
 
 bool saveCollectedPose(panda_teaching::SavePose::Request &req,
                        panda_teaching::SavePose::Response &res) {
 
-    x_transform.push_back((double) req.x_transform);
-    y_transform.push_back((double) req.y_transform);
-    z_transform.push_back((double) req.z_transform);
-
-    x_rotation.push_back((double) req.x_rotation);
-    y_rotation.push_back((double) req.y_rotation);
-    z_rotation.push_back((double) req.z_rotation);
-    w_rotation.push_back((double) req.w_rotation);
+    poses.push_back(req.pose);
 
     return true;
 }
@@ -41,14 +27,7 @@ bool saveCollectedPose(panda_teaching::SavePose::Request &req,
 bool deleteCollectedPoses(panda_teaching::DeletePose::Request &req,
                           panda_teaching::DeletePose::Response &res) {
 
-    x_transform.clear();
-    y_transform.clear();
-    z_transform.clear();
-
-    x_rotation.clear();
-    y_rotation.clear();
-    z_rotation.clear();
-    w_rotation.clear();
+    poses.clear();
 
     return true;
 }

src/teaching/pose_tf_listener.cpp

@@ -13,14 +13,14 @@ void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
     ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
     panda_teaching::SavePose srv;
 
-    srv.request.x_transform = t.transform.translation.x;
-    srv.request.y_transform = t.transform.translation.y;
-    srv.request.z_transform = t.transform.translation.z;
+    srv.request.pose.position.x = t.transform.translation.x;
+    srv.request.pose.position.y = t.transform.translation.y;
+    srv.request.pose.position.z = t.transform.translation.z;
 
-    srv.request.x_rotation = t.transform.rotation.x;
-    srv.request.y_rotation = t.transform.rotation.y;
-    srv.request.z_rotation = t.transform.rotation.z;
-    srv.request.w_rotation = t.transform.rotation.w;
+    srv.request.pose.orientation.x = t.transform.rotation.x;
+    srv.request.pose.orientation.y = t.transform.rotation.y;
+    srv.request.pose.orientation.z = t.transform.rotation.z;
+    srv.request.pose.orientation.w = t.transform.rotation.w;
 
     if (!client.call(srv)) {
         ROS_ERROR("Failed to call pose_storage_service.");

srv/GetCollectedPoses.srv

 ---
-float64[] x_transform
-float64[] y_transform
-float64[] z_transform
-float64[] x_rotation
-float64[] y_rotation
-float64[] z_rotation
-float64[] w_rotation
\ No newline at end of file
+geometry_msgs/PoseArray pose_array
\ No newline at end of file

srv/SavePose.srv

-float64 x_transform
-float64 y_transform
-float64 z_transform
-float64 x_rotation
-float64 y_rotation
-float64 z_rotation
-float64 w_rotation
+geometry_msgs/Pose pose
 ---
\ No newline at end of file