Skip to content
Snippets Groups Projects
Commit e90b0e7f authored by Johannes Mey's avatar Johannes Mey
Browse files

remove use of parameter

parent 5e1af023
No related branches found
No related tags found
No related merge requests found
......@@ -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;
};
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment