diff --git a/CMakeLists.txt b/CMakeLists.txt
index d99a7cdf830115fa2c5ed0571c853cda1b19d1d2..406747dd7dbdaad5f141800576b6d7aa7fc7c2be 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,7 @@ add_service_files(
generate_messages(
DEPENDENCIES
std_msgs
+ geometry_msgs
)
# ################################################################################################################################
diff --git a/src/teaching/pose_replayer.cpp b/src/teaching/pose_replayer.cpp
index c9dd564803914bdbe3e2ba6459d9a3cf53045e87..fe3fc08ebe0f81cee80d99f6ca40f872d17dfae3 100644
--- a/src/teaching/pose_replayer.cpp
+++ b/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;
diff --git a/src/teaching/pose_storage_service.cpp b/src/teaching/pose_storage_service.cpp
index 925b797ba07e9b794bdcdcb63dc73df8dfec662d..8505bad07c367fddfb15b1281ca5e6b3d07d0599 100644
--- a/src/teaching/pose_storage_service.cpp
+++ b/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;
}
diff --git a/src/teaching/pose_tf_listener.cpp b/src/teaching/pose_tf_listener.cpp
index f86eaa99ac39df83332552d1e29a4408ed9cee6a..35e5e1d5609c49a8c39c5aa5c0f144bb79d2d132 100644
--- a/src/teaching/pose_tf_listener.cpp
+++ b/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.");
diff --git a/srv/GetCollectedPoses.srv b/srv/GetCollectedPoses.srv
index fc924e4b310f29a28510dcf23c02216f81ce0022..1dd5ab5f63d99cea469ce8d62d0489c86b8c3663 100644
--- a/srv/GetCollectedPoses.srv
+++ b/srv/GetCollectedPoses.srv
@@ -1,8 +1,2 @@
---
-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
diff --git a/srv/SavePose.srv b/srv/SavePose.srv
index 542634afa5a77af2efbc7ba8bc94af7dc24a0b49..7cd0fe54c483b24818ad9d765d23a669fbe39c04 100644
--- a/srv/SavePose.srv
+++ b/srv/SavePose.srv
@@ -1,8 +1,2 @@
-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