Skip to content
Snippets Groups Projects
Commit 52273f48 authored by Sebastian Ebert's avatar Sebastian Ebert
Browse files

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

parent 3362e231
Branches
No related tags found
No related merge requests found
Pipeline #6788 passed
...@@ -23,6 +23,7 @@ add_service_files( ...@@ -23,6 +23,7 @@ add_service_files(
GetCollectedPoses.srv GetCollectedPoses.srv
SavePose.srv SavePose.srv
DeletePose.srv DeletePose.srv
ComputeError.srv
) )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
...@@ -38,6 +39,7 @@ generate_messages( ...@@ -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 # The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
catkin_package(CATKIN_DEPENDS catkin_package(CATKIN_DEPENDS
moveit_core moveit_core
moveit_visual_tools
moveit_ros_planning_interface moveit_ros_planning_interface
pluginlib pluginlib
) )
......
...@@ -14,6 +14,7 @@ ...@@ -14,6 +14,7 @@
<depend>moveit_core</depend> <depend>moveit_core</depend>
<depend>moveit_ros_planning_interface</depend> <depend>moveit_ros_planning_interface</depend>
<depend>moveit_visual_tools</depend>
<depend>robot_state_publisher</depend> <depend>robot_state_publisher</depend>
<depend>tf2</depend> <depend>tf2</depend>
<build_depend>message_generation</build_depend> <build_depend>message_generation</build_depend>
......
...@@ -10,6 +10,11 @@ ...@@ -10,6 +10,11 @@
#include <moveit/planning_scene_interface/planning_scene_interface.h> #include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "panda_teaching/GetCollectedPoses.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) { 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 ...@@ -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::MoveGroupInterface move_group(planningGroup);
moveit::planning_interface::PlanningSceneInterface planning_scene_interface; moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
std::vector<geometry_msgs::Pose> poses; std::vector<geometry_msgs::Pose> poses;
ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses"); 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 ...@@ -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++) { 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]; geometry_msgs::Pose another_pose = srv.response.pose_array.poses[i];
move_group.setPoseTarget(another_pose); move_group.setPoseTarget(another_pose);
...@@ -47,6 +51,15 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node ...@@ -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); bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED"); ROS_INFO_NAMED("pose_replayer", "Visualizing plan %s", success ? "" : "FAILED");
move_group.move(); 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 { } else {
ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data); ROS_ERROR_STREAM("pose_replayer received unknown panda_teaching/poseReplayer message " << msg->data);
......
// //
// Created by sebastian on 04.05.20. // Created by sebastian on 04.05.20.
// //
#include <stdlib.h>
#include <ros/ros.h> #include <ros/ros.h>
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/TransformStamped.h>
...@@ -8,6 +10,8 @@ ...@@ -8,6 +10,8 @@
#include "panda_teaching/SavePose.h" #include "panda_teaching/SavePose.h"
#include "panda_teaching/DeletePose.h" #include "panda_teaching/DeletePose.h"
#include "panda_teaching/ComputeError.h"
#include "panda_teaching/GetCollectedPoses.h"
// global variable. should be put in class // global variable. should be put in class
tf2_ros::Buffer tfBuffer; tf2_ros::Buffer tfBuffer;
...@@ -51,9 +55,12 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n ...@@ -51,9 +55,12 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n
if (msg->data.compare("add") == 0) { if (msg->data.compare("add") == 0) {
ROS_INFO_STREAM("Recording new pose for " << sourceFrame << "..."); ROS_INFO_STREAM("Recording new pose for " << sourceFrame << "...");
try { 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 << "Added pose: " << transformStamped << std::endl;
std::cout << "--------------------------------" << std::endl;
collect(node_handle, transformStamped); collect(node_handle, transformStamped);
} }
catch (tf2::TransformException &ex) { catch (tf2::TransformException &ex) {
...@@ -68,14 +75,61 @@ void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &n ...@@ -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) { int main(int argc, char **argv) {
ros::init(argc, argv, "pose_tf_listener"); ros::init(argc, argv, "pose_tf_listener");
ros::NodeHandle node_handle("panda_teaching"); 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::bind(&collectorCallback, _1,
boost::ref(node_handle))); 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); tf2_ros::TransformListener tfListener(tfBuffer);
ros::Rate rate(10.0); ros::Rate rate(10.0);
......
int32 poseNumber
---
\ No newline at end of file
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment