completet rework based on services, update readme

CMakeLists.txt

@@ -9,23 +9,17 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_C
 # Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other
 # catkin packages
 find_package(catkin REQUIRED
-             COMPONENTS controller_manager
-                        effort_controllers
-                        gazebo_ros
-                        joint_state_controller
-                        joint_state_publisher
-                        joint_trajectory_controller
+             COMPONENTS gazebo_ros
+                        controller_interface
+                        hardware_interface
                         robot_state_publisher
                         roscpp
                         std_msgs
                         tf
-                        xacro
                         moveit_core
                         moveit_visual_tools
                         moveit_ros_planning
                         moveit_ros_planning_interface
-                        controller_interface
-                        hardware_interface
                         message_generation)
 
 # System dependencies are found with CMake's conventions
@@ -38,6 +32,7 @@ add_service_files(
         FILES
         GetCollectedPoses.srv
         SavePose.srv
+        DeletePose.srv
 )
 
 ## Generate added messages and services with any dependencies listed here

README.md

@@ -15,14 +15,6 @@ source devel/setup.bash
 roslaunch panda_simulation simulation.launch
 ```
 
-Depending on your operating systems language you might need to export the numeric type so that rviz can read the floating point numbers in the robot model correctly:
-
-```
-export LC_NUMERIC="en_US.UTF-8"
-```
-Otherwise, the robot will appear in rviz in a collapsed state.
-
-
 #### Teaching poses
 
 You can teach the (simulated) robot several poses which can be replayed in the order there where teached.

package.xml

 <?xml version="1.0"?>
 <package format="2">
   <name>panda_teaching</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
   <description>The panda_teaching package</description>
 
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="sebastian@todo.todo">sebastian</maintainer>
-
+  <maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
+  <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
 
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
   <license>TODO</license>
 
   <depend>moveit_core</depend>
@@ -27,10 +24,7 @@
   <exec_depend>pluginlib</exec_depend>
   <buildtool_depend>catkin</buildtool_depend>
 
-
-  <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
-
   </export>
 </package>

src/teaching/pose_collector.cpp

@@ -4,40 +4,19 @@
 #include <ros/ros.h>
 #include <std_msgs/String.h>
 
+#include "panda_teaching/DeletePose.h"
+
 void collectPose(ros::NodeHandle &node_handle){
     node_handle.setParam("collect_pose", true);
 }
 
-void cleanUp(){
-    ros::NodeHandle node_handle;
-    std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
-
-    node_handle.getParam("x_transform", x_transform);
-    node_handle.getParam("y_transform", y_transform);
-    node_handle.getParam("z_transform", z_transform);
-
-    node_handle.getParam("x_rotation", x_rotation);
-    node_handle.getParam("y_rotation", y_rotation);
-    node_handle.getParam("z_rotation", z_rotation);
-    node_handle.getParam("w_rotation", w_rotation);
+void cleanUp(ros::NodeHandle &node_handle){
+    ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
+    panda_teaching::DeletePose srv;
 
-    x_transform.clear();
-    y_transform.clear();
-    z_transform.clear();
-    z_transform.clear();
-
-    x_rotation.clear();
-    y_rotation.clear();
-    z_rotation.clear();
-
-    node_handle.setParam("x_transform", x_transform);
-    node_handle.setParam("y_transform", y_transform);
-    node_handle.setParam("z_transform", z_transform);
-
-    node_handle.setParam("x_rotation", x_rotation);
-    node_handle.setParam("y_rotation", y_rotation);
-    node_handle.setParam("z_rotation", z_rotation);
-    node_handle.setParam("w_rotation", w_rotation);
+    if (!client.call(srv)){
+        ROS_ERROR("Failed to delete saved poses");
+    }
 }
 
 void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &node_handle){
@@ -52,7 +31,7 @@ void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &n
 
     if(msg->data.compare("clear") == 0){
         ROS_INFO("Deleting saved poses...");
-        cleanUp();
+        cleanUp(node_handle);
         return;
     }
 }

src/teaching/pose_replayer.cpp

@@ -11,6 +11,8 @@
 #include <moveit_visual_tools/moveit_visual_tools.h>
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
 
+#include "panda_teaching/GetCollectedPoses.h"
+
 void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
 
     static const std::string PLANNING_GROUP = "panda_arm";
@@ -32,14 +34,24 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
     visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
     visual_tools.trigger();
 
-    node_handle.getParam("x_transform", x_transform);
-    node_handle.getParam("y_transform", y_transform);
-    node_handle.getParam("z_transform", z_transform);
+    ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses");
+    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]);
 
-    node_handle.getParam("x_rotation", x_rotation);
-    node_handle.getParam("y_rotation", y_rotation);
-    node_handle.getParam("z_rotation", z_rotation);
-    node_handle.getParam("w_rotation", w_rotation);
+            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]);
+        }
+    } else {
+        ROS_ERROR("Failed to fetch saved poses");
+        return;
+    }
 
     for (int i = 0; i < x_transform.size(); i++) {
 
@@ -68,7 +80,9 @@ int main(int argc, char **argv) {
     ros::AsyncSpinner spinner(2);
     spinner.start();
 
-    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("replayPose", 1000, boost::bind(&replayCallback, _1, boost::ref(node_handle)));
+    ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("replayPose", 1000, boost::bind(&replayCallback, _1,
+                                                                                                  boost::ref(
+                                                                                                          node_handle)));
     ros::waitForShutdown();
     return 0;
 }

src/teaching/pose_storage_service.cpp

 #include "ros/ros.h"
 #include "panda_teaching/GetCollectedPoses.h"
 #include "panda_teaching/SavePose.h"
+#include "panda_teaching/DeletePose.h"
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/TransformStamped.h>
 #include <geometry_msgs/Twist.h>
 
 std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
 
-bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request  &req,
-             panda_teaching::GetCollectedPoses::Response &res){
+bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req,
+                       panda_teaching::GetCollectedPoses::Response &res) {
 
     res.x_transform = x_transform;
     res.y_transform = y_transform;
@@ -22,32 +23,47 @@ bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request  &req,
     return true;
 }
 
-bool saveCollectedPose(panda_teaching::SavePose::Request  &req,
-         panda_teaching::SavePose::Response &res){
+bool saveCollectedPose(panda_teaching::SavePose::Request &req,
+                       panda_teaching::SavePose::Response &res) {
 
-  std::cout << "retrieved data: " << req << std::endl;
-  x_transform.push_back((long) req.x_transform);
-  y_transform.push_back((long) req.y_transform);
-  z_transform.push_back((long) req.z_transform);
+    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((long) req.x_rotation);
-  y_rotation.push_back((long) req.y_rotation);
-  z_rotation.push_back((long) req.z_rotation);
-  w_rotation.push_back((long) req.w_rotation);
+    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);
 
-  return true;
+    return true;
+}
+
+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();
+
+    return true;
 }
 
-int main(int argc, char **argv){
+int main(int argc, char **argv) {
 
-  ros::init(argc, argv, "pose_service_service");
-  ros::NodeHandle n;
+    ros::init(argc, argv, "pose_service_service");
+    ros::NodeHandle n;
 
-  ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses);
-  ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose);
+    ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses);
+    ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose);
+    ros::ServiceServer delete_service = n.advertiseService("DeleteCollectedPoses", deleteCollectedPoses);
 
-  ROS_INFO("Ready to collect.");
-  ros::spin();
+    ROS_INFO("Ready to collect.");
+    ros::spin();
 
-  return 0;
+    return 0;
 }

src/teaching/pose_tf_listener.cpp

@@ -8,7 +8,7 @@
 
 #include "panda_teaching/SavePose.h"
 
-void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t){
+void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
 
     ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
     panda_teaching::SavePose srv;
@@ -22,13 +22,13 @@ void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t){
     srv.request.z_rotation = t.transform.rotation.z;
     srv.request.w_rotation = t.transform.rotation.w;
 
-    if (!client.call(srv)){
+    if (!client.call(srv)) {
         ROS_ERROR("Failed to call pose_storage_service.");
         return;
     }
 }
 
-int main(int argc, char** argv) {
+int main(int argc, char **argv) {
     ros::init(argc, argv, "pose_tf_listener");
 
     ros::NodeHandle node_handle;
@@ -38,11 +38,11 @@ int main(int argc, char** argv) {
 
     ros::Rate rate(10.0);
 
-    while (node_handle.ok()){
+    while (node_handle.ok()) {
 
         geometry_msgs::TransformStamped transformStamped;
 
-        try{
+        try {
             // world == (0, 0, 0, 0)
             transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
                                                         ros::Time(0));
@@ -50,7 +50,7 @@ int main(int argc, char** argv) {
             bool collect_pose = false;
             node_handle.getParam("collect_pose", collect_pose);
 
-            if(collect_pose){
+            if (collect_pose) {
                 std::cout << "Collected pose: " << transformStamped << std::endl;
                 node_handle.setParam("collect_pose", false);
                 collect(node_handle, transformStamped);

srv/DeletePose.srv

+---
+bool success