diff --git a/CMakeLists.txt b/CMakeLists.txt index d7792186d39bf8f9d1ab4c602e0da818df281334..d99a7cdf830115fa2c5ed0571c853cda1b19d1d2 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/README.md b/README.md index 5cae0a4ae58a716f194be3d8e9fc9ff47891f0f6..2d845ad02f6acf04856958fda0a307c7d02df46c 100644 --- a/README.md +++ b/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. diff --git a/package.xml b/package.xml index bde598bd6f163d2957865a22eb81a95545e0e4b4..75142d834594de02335b72220b64fcfa6e24e156 100644 --- a/package.xml +++ b/package.xml @@ -1,18 +1,15 @@ <?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> diff --git a/src/teaching/pose_collector.cpp b/src/teaching/pose_collector.cpp index f052d6a890ce0a3e3d89047ed83ac67163074465..d460d058fb70de865bd0acfa9cef6cdb46ced35b 100644 --- a/src/teaching/pose_collector.cpp +++ b/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; } } diff --git a/src/teaching/pose_replayer.cpp b/src/teaching/pose_replayer.cpp index f1844e3b6c032a36b9bda86ecc0778ebb7ff0a4a..c9dd564803914bdbe3e2ba6459d9a3cf53045e87 100644 --- a/src/teaching/pose_replayer.cpp +++ b/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; } diff --git a/src/teaching/pose_storage_service.cpp b/src/teaching/pose_storage_service.cpp index 1c594941de94e3bd709f9f6971004b7494aee30a..925b797ba07e9b794bdcdcb63dc73df8dfec662d 100644 --- a/src/teaching/pose_storage_service.cpp +++ b/src/teaching/pose_storage_service.cpp @@ -1,14 +1,15 @@ #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; } diff --git a/src/teaching/pose_tf_listener.cpp b/src/teaching/pose_tf_listener.cpp index 62dde33931ae845e61439163106e21423707f97d..f86eaa99ac39df83332552d1e29a4408ed9cee6a 100644 --- a/src/teaching/pose_tf_listener.cpp +++ b/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); diff --git a/srv/DeletePose.srv b/srv/DeletePose.srv new file mode 100644 index 0000000000000000000000000000000000000000..410e0f9d9d2a1bd456293e81e81e615b3abb4268 --- /dev/null +++ b/srv/DeletePose.srv @@ -0,0 +1,2 @@ +--- +bool success