diff --git a/src/teaching/pose_tf_listener.cpp b/src/teaching/pose_tf_listener.cpp index 85d73cb07148abd88e6f3fbc243d203cdee153b5..b66c3140bf94543720e18cea5fa897399c81e908 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; +};