From 52273f48c70a59fb1e74c5592ca3a23932c21a7a Mon Sep 17 00:00:00 2001 From: SebastianEbert <sebastian.ebert@tu-dresden.de> Date: Tue, 26 May 2020 17:47:31 +0200 Subject: [PATCH] fixed bug which caused incorrect replayed positions, added error-logging for poses --- CMakeLists.txt | 2 + package.xml | 1 + src/teaching/pose_replayer.cpp | 101 +++++++++++---------- src/teaching/pose_tf_listener.cpp | 140 +++++++++++++++++++++--------- srv/ComputeError.srv | 2 + 5 files changed, 159 insertions(+), 87 deletions(-) create mode 100644 srv/ComputeError.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 89e6a9b..f738ed6 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 4864638..45f3ee8 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 3e596ae..4d6810a 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 4c02857..ade3550 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 0000000..4ba4819 --- /dev/null +++ b/srv/ComputeError.srv @@ -0,0 +1,2 @@ +int32 poseNumber +--- \ No newline at end of file -- GitLab