diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89e6a9bef58fc385277aa9e1f6ed60d9de75f5c0..f738ed6ca85d8d579b190601eedef6828494d636 100644
--- a/CMakeLists.txt
+++ b/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
)
diff --git a/package.xml b/package.xml
index 48646385d541bf93e3f91db77ad04fa9c7a768fb..45f3ee82dbee8bc5eac80bc05b504fa294bdd574 100644
--- a/package.xml
+++ b/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>
diff --git a/src/teaching/pose_replayer.cpp b/src/teaching/pose_replayer.cpp
index 3e596aec3439459c3a670de6bf9d3c2b126ac2c8..4d6810a6d50c7cf280d85ef0df969dee8802abc3 100644
--- a/src/teaching/pose_replayer.cpp
+++ b/src/teaching/pose_replayer.cpp
@@ -10,60 +10,73 @@
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "panda_teaching/GetCollectedPoses.h"
+#include "panda_teaching/ComputeError.h"
-void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
-
- if (msg->data.compare("replay") == 0) {
- std::string planningGroup;
- if (!node_handle.getParam("planning_group", planningGroup)) {
- ROS_ERROR("Could not get string value for panda_teaching/planning_group from param server");
- return;
- }
+void printError(){
- 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");
- panda_teaching::GetCollectedPoses srv;
+void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
- if (client.call(srv)) {
- for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
- poses.push_back(srv.response.pose_array.poses[i]);
- }
+ if (msg->data.compare("replay") == 0) {
+ std::string planningGroup;
+ if (!node_handle.getParam("planning_group", planningGroup)) {
+ ROS_ERROR("Could not get string value for panda_teaching/planning_group from param server");
+ return;
+ }
+
+ 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");
+ panda_teaching::GetCollectedPoses srv;
+
+ if (client.call(srv)) {
+ 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 < srv.response.pose_array.poses.size(); i++) {
+
+ //move_group.setStartState(*move_group.getCurrentState());
+ geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
+ move_group.setPoseTarget(another_pose);
+
+ moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+ 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("Failed to fetch saved poses");
- return;
- }
-
- for (int i = 0; i < srv.response.pose_array.poses.size(); i++) {
-
- move_group.setStartState(*move_group.getCurrentState());
- geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
- move_group.setPoseTarget(another_pose);
-
- moveit::planning_interface::MoveGroupInterface::Plan my_plan;
- 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_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data);
}
- } else {
- ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data);
- }
}
int main(int argc, char **argv) {
- ros::init(argc, argv, "pose_replayer");
- ros::NodeHandle node_handle("panda_teaching");
+ ros::init(argc, argv, "pose_replayer");
+ ros::NodeHandle node_handle("panda_teaching");
- ros::AsyncSpinner spinner(2);
- spinner.start();
+ ros::AsyncSpinner spinner(2);
+ spinner.start();
- ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseReplayer", 1000, boost::bind(&replayCallback, _1,
- boost::ref(
- node_handle)));
- ros::waitForShutdown();
- return 0;
+ ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseReplayer", 1000, boost::bind(&replayCallback, _1,
+ boost::ref(
+ node_handle)));
+ ros::waitForShutdown();
+ return 0;
}
diff --git a/src/teaching/pose_tf_listener.cpp b/src/teaching/pose_tf_listener.cpp
index 4c02857117bbfa7c03243bffb81869599342882b..ade35501fcf04e537b84e05e75215c23c7fb8c1a 100644
--- a/src/teaching/pose_tf_listener.cpp
+++ b/src/teaching/pose_tf_listener.cpp
@@ -1,6 +1,8 @@
//
// 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,78 +10,130 @@
#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;
void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t) {
- ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
- panda_teaching::SavePose srv;
+ ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
+ panda_teaching::SavePose srv;
- 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.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.pose.orientation = t.transform.rotation;
+ srv.request.pose.orientation = t.transform.rotation;
- if (!client.call(srv)) {
- ROS_ERROR("Failed to call pose_storage_service.");
- return;
- }
+ if (!client.call(srv)) {
+ ROS_ERROR("Failed to call pose_storage_service.");
+ return;
+ }
}
void cleanUp(ros::NodeHandle &node_handle) {
- ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
- panda_teaching::DeletePose srv;
+ ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
+ panda_teaching::DeletePose srv;
- if (!client.call(srv)) {
- ROS_ERROR("Failed to delete saved poses");
- }
+ if (!client.call(srv)) {
+ ROS_ERROR("Failed to delete saved poses");
+ }
}
void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
- ROS_INFO("Retrieved new pose-message...");
+ ROS_INFO("Retrieved new pose-message...");
+
+ 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;
+ }
+
+ 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));
+
+ std::cout << "--------------------------------" << std::endl;
+ std::cout << "Added pose: " << transformStamped << std::endl;
+ std::cout << "--------------------------------" << std::endl;
+ collect(node_handle, transformStamped);
+ }
+ catch (tf2::TransformException &ex) {
+ ROS_ERROR_STREAM("transform_error: " << ex.what() << std::endl);
+ ros::Duration(1.0).sleep();
+ }
+ } else if (msg->data.compare("clear") == 0) {
+ ROS_INFO_STREAM("Deleting saved poses...");
+ cleanUp(node_handle);
+ } else {
+ ROS_ERROR_STREAM("pose_tf_listener received unknown panda_teaching/poseRecorder message " << msg->data);
+ }
+}
+
+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;
- }
+ 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;
+ }
- 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));
+ 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;
+ }
- std::cout << "Added pose: " << transformStamped << std::endl;
- collect(node_handle, transformStamped);
}
catch (tf2::TransformException &ex) {
- ROS_ERROR_STREAM("transform_error: " << ex.what() << std::endl);
- ros::Duration(1.0).sleep();
+ ROS_ERROR_STREAM("transform_error: " << ex.what() << std::endl);
+ ros::Duration(1.0).sleep();
+ return false;
}
- } else if (msg->data.compare("clear") == 0) {
- ROS_INFO_STREAM("Deleting saved poses...");
- cleanUp(node_handle);
- } else {
- ROS_ERROR_STREAM("pose_tf_listener received unknown panda_teaching/poseRecorder message " << msg->data);
- }
+ return true;
}
int main(int argc, char **argv) {
- ros::init(argc, argv, "pose_tf_listener");
- ros::NodeHandle node_handle("panda_teaching");
+ ros::init(argc, argv, "pose_tf_listener");
+ ros::NodeHandle node_handle("panda_teaching");
+
+ 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)));
- ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("poseRecorder", 1000,
- boost::bind(&collectorCallback, _1,
- boost::ref(node_handle)));
- tf2_ros::TransformListener tfListener(tfBuffer);
+ tf2_ros::TransformListener tfListener(tfBuffer);
- ros::Rate rate(10.0);
+ ros::Rate rate(10.0);
- ros::spin();
- return 0;
+ ros::spin();
+ return 0;
};
diff --git a/srv/ComputeError.srv b/srv/ComputeError.srv
new file mode 100644
index 0000000000000000000000000000000000000000..4ba4819c232a852e89188a89deac86c73b7de4dc
--- /dev/null
+++ b/srv/ComputeError.srv
@@ -0,0 +1,2 @@
+int32 poseNumber
+---
\ No newline at end of file