From e90b0e7f172fedeaa7f01534903f36bf58e22581 Mon Sep 17 00:00:00 2001
From: Johannes Mey <johannes.mey@tu-dresden.de>
Date: Mon, 11 May 2020 19:23:21 +0200
Subject: [PATCH] remove use of parameter
---
src/teaching/pose_tf_listener.cpp | 88 ++++++++++++-------------------
1 file changed, 35 insertions(+), 53 deletions(-)
diff --git a/src/teaching/pose_tf_listener.cpp b/src/teaching/pose_tf_listener.cpp
index 85d73cb..b66c314 100644
--- a/src/teaching/pose_tf_listener.cpp
+++ b/src/teaching/pose_tf_listener.cpp
@@ -12,45 +12,53 @@
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;
- }
-}
-
-void collectPose(ros::NodeHandle &node_handle){
- node_handle.setParam("collect_pose", true);
+ if (!client.call(srv)) {
+ ROS_ERROR("Failed to call pose_storage_service.");
+ return;
+ }
}
-void cleanUp(ros::NodeHandle &node_handle){
+void cleanUp(ros::NodeHandle &node_handle) {
ros::ServiceClient client = node_handle.serviceClient<panda_teaching::DeletePose>("DeleteCollectedPoses");
panda_teaching::DeletePose srv;
- if (!client.call(srv)){
+ if (!client.call(srv)) {
ROS_ERROR("Failed to delete saved poses");
}
}
-void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &node_handle){
+// global variable. should be put in class
+tf2_ros::Buffer tfBuffer;
+
+void collectorCallback(const std_msgs::String::ConstPtr &msg, ros::NodeHandle &node_handle) {
ROS_INFO("Retrieved new pose-message...");
- if(msg->data.compare("collect") == 0){
+ if (msg->data.compare("collect") == 0) {
ROS_INFO("Collecting new pose for gripper...");
- collectPose(node_handle);
+ try {
+ geometry_msgs::TransformStamped transformStamped = tfBuffer.lookupTransform("world", "panda_hand", ros::Time(0));
+
+ std::cout << "Collected 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();
+ }
return;
}
- if(msg->data.compare("clear") == 0){
+ if (msg->data.compare("clear") == 0) {
ROS_INFO("Deleting saved poses...");
cleanUp(node_handle);
return;
@@ -58,42 +66,16 @@ void collectorCallback(const std_msgs::String::ConstPtr& msg, ros::NodeHandle &n
}
int main(int argc, char **argv) {
- ros::init(argc, argv, "pose_tf_listener");
-
- ros::NodeHandle node_handle;
+ ros::init(argc, argv, "pose_tf_listener");
+ ros::NodeHandle node_handle;
ros::Subscriber sub = node_handle.subscribe<std_msgs::String>("collectPose", 1000, boost::bind(&collectorCallback, _1, boost::ref(node_handle)));
- tf2_ros::Buffer tfBuffer;
- tf2_ros::TransformListener tfListener(tfBuffer);
-
- ros::Rate rate(10.0);
-
- while (node_handle.ok()) {
+ tf2_ros::TransformListener tfListener(tfBuffer);
- geometry_msgs::TransformStamped transformStamped;
+ ros::Rate rate(10.0);
- try {
- // world == (0, 0, 0, 0)
- transformStamped = tfBuffer.lookupTransform("world", "panda_hand",
- ros::Time(0));
-
- bool collect_pose = false;
- node_handle.getParam("collect_pose", collect_pose);
-
- if (collect_pose) {
- std::cout << "Collected pose: " << transformStamped << std::endl;
- node_handle.setParam("collect_pose", false);
- collect(node_handle, transformStamped);
- }
- }
- catch (tf2::TransformException &ex) {
- std::cout << "transform_error: " << ex.what() << std::endl;
- ros::Duration(1.0).sleep();
- continue;
- }
- ros::spinOnce();
- }
- return 0;
-};
\ No newline at end of file
+ ros::spin();
+ return 0;
+};
--
GitLab