fixed bug which caused incorrect replayed positions, added error-logging for poses

CMakeLists.txt

@@ -23,6 +23,7 @@ add_service_files(
         GetCollectedPoses.srv
         SavePose.srv
         DeletePose.srv
+        ComputeError.srv
 )
 
 ## Generate added messages and services with any dependencies listed here
@@ -38,6 +39,7 @@ generate_messages(
 # The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
 catkin_package(CATKIN_DEPENDS
                moveit_core
+               moveit_visual_tools
                moveit_ros_planning_interface
                pluginlib
         )

package.xml

@@ -14,6 +14,7 @@
 
   <depend>moveit_core</depend>
   <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_visual_tools</depend>
   <depend>robot_state_publisher</depend>
   <depend>tf2</depend>
   <build_depend>message_generation</build_depend>

src/teaching/pose_replayer.cpp

@@ -10,6 +10,11 @@
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 
 #include "panda_teaching/GetCollectedPoses.h"
+#include "panda_teaching/ComputeError.h"
+
+void printError(){
+
+}
 
 void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
 
@@ -22,7 +27,6 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
 
         moveit::planning_interface::MoveGroupInterface move_group(planningGroup);
         moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
         std::vector<geometry_msgs::Pose> poses;
 
         ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses");
@@ -39,7 +43,7 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
 
         for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
 
-      move_group.setStartState(*move_group.getCurrentState());
+            //move_group.setStartState(*move_group.getCurrentState());
             geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
             move_group.setPoseTarget(another_pose);
 
@@ -47,6 +51,15 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node
             bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
             ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
             move_group.move();
+
+            ros::ServiceClient client = node_handle.serviceClient<panda_teaching::ComputeError>("errorChecker");
+            panda_teaching::ComputeError srv;
+            srv.request.poseNumber = i;
+
+            if (!client.call(srv)) {
+                ROS_ERROR("Failed to call errorChecker.");
+                return;
+            }
         }
     } else {
         ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data);

src/teaching/pose_tf_listener.cpp

 //
 // Created by sebastian on 04.05.20.
 //
+#include <stdlib.h>
+
 #include <ros/ros.h>
 #include <tf2_ros/transform_listener.h>
 #include <geometry_msgs/TransformStamped.h>
@@ -8,6 +10,8 @@
 
 #include "panda_teaching/SavePose.h"
 #include "panda_teaching/DeletePose.h"
+#include "panda_teaching/ComputeError.h"
+#include "panda_teaching/GetCollectedPoses.h"
 
 // global variable. should be put in class
 tf2_ros::Buffer tfBuffer;
@@ -51,9 +55,12 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n
     if (msg->data.compare("add") == 0) {
         ROS_INFO_STREAM("Recording new pose for " << sourceFrame << "...");
         try {
-      geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", sourceFrame, ros::Time(0));
+            geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", sourceFrame,
+                                                                                        ros::Time(0));
 
+            std::cout << "--------------------------------" << std::endl;
             std::cout << "Added pose: " << transformStamped << std::endl;
+            std::cout << "--------------------------------" << std::endl;
             collect(node_handle, transformStamped);
         }
         catch (tf2::TransformException &ex) {
@@ -68,14 +75,61 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n
     }
 }
 
+bool errorComputationCallback(panda_teaching::ComputeError::Request &req,
+                   panda_teaching::ComputeError::Response &res, ros::NodeHandle &node_handle) {
+
+    std::string sourceFrame;
+    if (!node_handle.getParam("pose_element", sourceFrame)) {
+        ROS_ERROR("Could not get string value for panda_teaching/pose_element from param server");
+        return false;
+    }
+
+    try {
+        geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", sourceFrame,
+                                                                                    ros::Time(0));
+        int poseNumber = req.poseNumber;
+        geometry_msgs::Pose currentTargetPose;
+
+        ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses");
+        panda_teaching::GetCollectedPoses srv;
+
+        if (client.call(srv)) {
+            currentTargetPose = srv.response.pose_array.poses[poseNumber];
+
+            std::cout << "position-error (xyz): " << (std::abs(transformStamped.transform.translation.x) -std::abs( currentTargetPose.position.x)) << " -- "
+                    << (std::abs(transformStamped.transform.translation.y) - std::abs(currentTargetPose.position.y)) << " -- "
+                    << (std::abs(transformStamped.transform.translation.z) - std::abs(currentTargetPose.position.z)) << " -- " << std::endl;
+
+            std::cout << "orientation (wxyz): " << (std::abs(transformStamped.transform.rotation.w) - std::abs(currentTargetPose.orientation.w)) << " -- "
+                    << (std::abs(transformStamped.transform.rotation.x) - std::abs(currentTargetPose.orientation.x)) << " -- "
+                    << (std::abs(transformStamped.transform.rotation.y) - std::abs(currentTargetPose.orientation.y)) << " -- "
+                    << (std::abs(transformStamped.transform.rotation.z) - std::abs(currentTargetPose.orientation.z)) << " -- " << std::endl;
+        } else {
+            ROS_ERROR("Failed to fetch saved poses");
+            return false;
+        }
+
+    }
+    catch (tf2::TransformException &ex) {
+        ROS_ERROR_STREAM("transform_error: " << ex.what() << std::endl);
+        ros::Duration(1.0).sleep();
+        return false;
+    }
+    return true;
+}
+
 int main(int argc, char **argv) {
     ros::init(argc, argv, "pose_tf_listener");
     ros::NodeHandle node_handle("panda_teaching");
 
-  ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseRecorder", 1000,
+    ros::Subscriber recorder_service = node_handle.subscribe<std_msgs::String>("poseRecorder", 1000,
                                                                   boost::bind(&collectorCallback, _1,
                                                                               boost::ref(node_handle)));
 
+    ros::ServiceServer error_service = node_handle.advertiseService<panda_teaching::ComputeError::Request, panda_teaching::ComputeError::Response>("errorChecker",
+                                        boost::bind(&errorComputationCallback, _1, _2, boost::ref(node_handle)));
+
+
     tf2_ros::TransformListener tfListener(tfBuffer);
 
     ros::Rate rate(10.0);

srv/ComputeError.srv

+int32 poseNumber
+---
\ No newline at end of file