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

completet rework based on services, update readme

parent 1bfe8aae
No related branches found
No related tags found
No related merge requests found
...@@ -9,23 +9,17 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_C ...@@ -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 # Find catkin macros and libraries if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) is used, also find other
# catkin packages # catkin packages
find_package(catkin REQUIRED find_package(catkin REQUIRED
COMPONENTS controller_manager COMPONENTS gazebo_ros
effort_controllers controller_interface
gazebo_ros hardware_interface
joint_state_controller
joint_state_publisher
joint_trajectory_controller
robot_state_publisher robot_state_publisher
roscpp roscpp
std_msgs std_msgs
tf tf
xacro
moveit_core moveit_core
moveit_visual_tools moveit_visual_tools
moveit_ros_planning moveit_ros_planning
moveit_ros_planning_interface moveit_ros_planning_interface
controller_interface
hardware_interface
message_generation) message_generation)
# System dependencies are found with CMake's conventions # System dependencies are found with CMake's conventions
...@@ -38,6 +32,7 @@ add_service_files( ...@@ -38,6 +32,7 @@ add_service_files(
FILES FILES
GetCollectedPoses.srv GetCollectedPoses.srv
SavePose.srv SavePose.srv
DeletePose.srv
) )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
......
...@@ -15,14 +15,6 @@ source devel/setup.bash ...@@ -15,14 +15,6 @@ source devel/setup.bash
roslaunch panda_simulation simulation.launch 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 #### Teaching poses
You can teach the (simulated) robot several poses which can be replayed in the order there where teached. You can teach the (simulated) robot several poses which can be replayed in the order there where teached.
......
<?xml version="1.0"?> <?xml version="1.0"?>
<package format="2"> <package format="2">
<name>panda_teaching</name> <name>panda_teaching</name>
<version>0.0.0</version> <version>0.1.0</version>
<description>The panda_teaching package</description> <description>The panda_teaching package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag --> <maintainer email="sebastian.ebert@tu-dresden.de">Sebastian Ebert</maintainer>
<!-- Example: --> <maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</maintainer>
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="sebastian@todo.todo">sebastian</maintainer>
<!-- One license tag required, multiple allowed, one license per tag --> <!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: --> <!-- 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> <license>TODO</license>
<depend>moveit_core</depend> <depend>moveit_core</depend>
...@@ -27,10 +24,7 @@ ...@@ -27,10 +24,7 @@
<exec_depend>pluginlib</exec_depend> <exec_depend>pluginlib</exec_depend>
<buildtool_depend>catkin</buildtool_depend> <buildtool_depend>catkin</buildtool_depend>
<!-- The export tag contains other, unspecified, tags -->
<export> <export>
<!-- Other tools can request additional information be placed here --> <!-- Other tools can request additional information be placed here -->
</export> </export>
</package> </package>
...@@ -4,40 +4,19 @@ ...@@ -4,40 +4,19 @@
#include <ros/ros.h> #include <ros/ros.h>
#include <std_msgs/String.h> #include <std_msgs/String.h>
#include "panda_teaching/DeletePose.h"
void collectPose(ros::NodeHandle &node_handle){ void collectPose(ros::NodeHandle &node_handle){
node_handle.setParam("collect_pose", true); node_handle.setParam("collect_pose", true);
} }
void cleanUp(){ void cleanUp(ros::NodeHandle &node_handle){
ros::NodeHandle node_handle; ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation; panda_teaching::DeletePose srv;
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);
x_transform.clear(); if (!client.call(srv)){
y_transform.clear(); ROS_ERROR("Failed to delete saved poses");
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);
} }
void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &node_handle){ 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 ...@@ -52,7 +31,7 @@ void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &n
if(msg->data.compare("clear") == 0){ if(msg->data.compare("clear") == 0){
ROS_INFO("Deleting saved poses..."); ROS_INFO("Deleting saved poses...");
cleanUp(); cleanUp(node_handle);
return; return;
} }
} }
......
...@@ -11,6 +11,8 @@ ...@@ -11,6 +11,8 @@
#include <moveit_visual_tools/moveit_visual_tools.h> #include <moveit_visual_tools/moveit_visual_tools.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.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) { void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
static const std::string PLANNING_GROUP = "panda_arm"; static const std::string PLANNING_GROUP = "panda_arm";
...@@ -32,14 +34,24 @@ void replayCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node ...@@ -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.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger(); visual_tools.trigger();
node_handle.getParam("x_transform", x_transform); ros::ServiceClient client = node_handle.serviceClient<panda_teaching::GetCollectedPoses>("GetCollectedPoses");
node_handle.getParam("y_transform", y_transform); panda_teaching::GetCollectedPoses srv;
node_handle.getParam("z_transform", z_transform);
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); x_rotation.push_back((double) srv.response.x_rotation[i]);
node_handle.getParam("y_rotation", y_rotation); y_rotation.push_back((double) srv.response.y_rotation[i]);
node_handle.getParam("z_rotation", z_rotation); z_rotation.push_back((double) srv.response.z_rotation[i]);
node_handle.getParam("w_rotation", w_rotation); 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++) { for (int i = 0; i < x_transform.size(); i++) {
...@@ -68,7 +80,9 @@ int main(int argc, char **argv) { ...@@ -68,7 +80,9 @@ int main(int argc, char **argv) {
ros::AsyncSpinner spinner(2); ros::AsyncSpinner spinner(2);
spinner.start(); 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(); ros::waitForShutdown();
return 0; return 0;
} }
#include "ros/ros.h" #include "ros/ros.h"
#include "panda_teaching/GetCollectedPoses.h" #include "panda_teaching/GetCollectedPoses.h"
#include "panda_teaching/SavePose.h" #include "panda_teaching/SavePose.h"
#include "panda_teaching/DeletePose.h"
#include <tf2_ros/transform_listener.h> #include <tf2_ros/transform_listener.h>
#include <geometry_msgs/TransformStamped.h> #include <geometry_msgs/TransformStamped.h>
#include <geometry_msgs/Twist.h> #include <geometry_msgs/Twist.h>
std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation; std::vector<double> x_transform, y_transform, z_transform, x_rotation, y_rotation, z_rotation, w_rotation;
bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req, bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req,
panda_teaching::GetCollectedPoses::Response &res){ panda_teaching::GetCollectedPoses::Response &res) {
res.x_transform = x_transform; res.x_transform = x_transform;
res.y_transform = y_transform; res.y_transform = y_transform;
...@@ -22,32 +23,47 @@ bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req, ...@@ -22,32 +23,47 @@ bool getCollectedPoses(panda_teaching::GetCollectedPoses::Request &req,
return true; return true;
} }
bool saveCollectedPose(panda_teaching::SavePose::Request &req, bool saveCollectedPose(panda_teaching::SavePose::Request &req,
panda_teaching::SavePose::Response &res){ panda_teaching::SavePose::Response &res) {
std::cout << "retrieved data: " << req << std::endl; x_transform.push_back((double) req.x_transform);
x_transform.push_back((long) req.x_transform); y_transform.push_back((double) req.y_transform);
y_transform.push_back((long) req.y_transform); z_transform.push_back((double) req.z_transform);
z_transform.push_back((long) req.z_transform);
x_rotation.push_back((long) req.x_rotation); x_rotation.push_back((double) req.x_rotation);
y_rotation.push_back((long) req.y_rotation); y_rotation.push_back((double) req.y_rotation);
z_rotation.push_back((long) req.z_rotation); z_rotation.push_back((double) req.z_rotation);
w_rotation.push_back((long) req.w_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::init(argc, argv, "pose_service_service");
ros::NodeHandle n; ros::NodeHandle n;
ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses); ros::ServiceServer retrieve_service = n.advertiseService("GetCollectedPoses", getCollectedPoses);
ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose); ros::ServiceServer save_service = n.advertiseService("SaveCollectedPose", saveCollectedPose);
ros::ServiceServer delete_service = n.advertiseService("DeleteCollectedPoses", deleteCollectedPoses);
ROS_INFO("Ready to collect."); ROS_INFO("Ready to collect.");
ros::spin(); ros::spin();
return 0; return 0;
} }
...@@ -8,7 +8,7 @@ ...@@ -8,7 +8,7 @@
#include "panda_teaching/SavePose.h" #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"); ros::ServiceClient client = node_handle.serviceClient<panda_teaching::SavePose>("SaveCollectedPose");
panda_teaching::SavePose srv; panda_teaching::SavePose srv;
...@@ -22,13 +22,13 @@ void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t){ ...@@ -22,13 +22,13 @@ void collect(ros::NodeHandle node_handle, geometry_msgs::TransformStamped t){
srv.request.z_rotation = t.transform.rotation.z; srv.request.z_rotation = t.transform.rotation.z;
srv.request.w_rotation = t.transform.rotation.w; srv.request.w_rotation = t.transform.rotation.w;
if (!client.call(srv)){ if (!client.call(srv)) {
ROS_ERROR("Failed to call pose_storage_service."); ROS_ERROR("Failed to call pose_storage_service.");
return; return;
} }
} }
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; ros::NodeHandle node_handle;
...@@ -38,11 +38,11 @@ int main(int argc, char** argv) { ...@@ -38,11 +38,11 @@ int main(int argc, char** argv) {
ros::Rate rate(10.0); ros::Rate rate(10.0);
while (node_handle.ok()){ while (node_handle.ok()) {
geometry_msgs::TransformStamped transformStamped; geometry_msgs::TransformStamped transformStamped;
try{ try {
// world == (0, 0, 0, 0) // world == (0, 0, 0, 0)
transformStamped = tfBuffer.lookupTransform("world", "panda_hand", transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
ros::Time(0)); ros::Time(0));
...@@ -50,7 +50,7 @@ int main(int argc, char** argv) { ...@@ -50,7 +50,7 @@ int main(int argc, char** argv) {
bool collect_pose = false; bool collect_pose = false;
node_handle.getParam("collect_pose", collect_pose); node_handle.getParam("collect_pose", collect_pose);
if(collect_pose){ if (collect_pose) {
std::cout << "Collected pose: " << transformStamped << std::endl; std::cout << "Collected pose: " << transformStamped << std::endl;
node_handle.setParam("collect_pose", false); node_handle.setParam("collect_pose", false);
collect(node_handle, transformStamped); collect(node_handle, transformStamped);
......
---
bool success
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment