Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00049
00050 #include <ros/ros.h>
00051 #include <pcl/common/angles.h>
00052
00053 #include <tf/transform_listener.h>
00054 #include <kinect_cleanup/GetReleasedObject.h>
00055 #include <kinect_cleanup/ReleaseObject.h>
00056
00057
00058 #include <pcl_ros/transforms.h>
00059
00060
00061 #include <vision_msgs/cop_answer.h>
00062
00063 using namespace std;
00064
00065 class ObjectReleaser
00066 {
00067 protected:
00068 ros::NodeHandle nh_;
00069
00070 public:
00071 tf::TransformListener tf_;
00072 std::string world_, output_cloud_topic_, object_frame_, template_topic_;
00073 double tf_buffer_time_;
00074 bool got_object_;
00075 sensor_msgs::PointCloud2 output_cloud_;
00076 ros::Publisher pub_, template_publisher_;
00077 ros::ServiceClient client_get_released_object;
00078 ros::ServiceServer service;
00079 int object_id_;
00081 ObjectReleaser (ros::NodeHandle &n) : nh_(n)
00082 {
00083 nh_.param("world", world_, std::string("openni_rgb_optical_frame"));
00084 nh_.param("object_frame", object_frame_, std::string("right_hand"));
00085 nh_.param("output_cloud_topic_", output_cloud_topic_, std::string("/moved_object"));
00086 nh_.param("tf_buffer_time", tf_buffer_time_, 0.05);
00087
00088 nh_.param ("object_id", object_id_, 700000);
00089 nh_.param ("template_topic", template_topic_, std::string("object"));
00090 service = nh_.advertiseService("/release_object", &ObjectReleaser::calculate_transform, this);
00091 pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_topic_, 1);
00092 client_get_released_object = nh_.serviceClient<kinect_cleanup::GetReleasedObject>("/get_released_object");
00093 template_publisher_ = nh_.advertise<vision_msgs::cop_answer>(template_topic_, 1);
00094 got_object_ = false;
00095 }
00096
00097
00099
00100 void get_object(sensor_msgs::PointCloud2 &cloud)
00101 {
00102 kinect_cleanup::GetReleasedObject srv_rel_obj;
00103 srv_rel_obj.request.object_released = true;
00104 if (client_get_released_object.call (srv_rel_obj))
00105 {
00106 cloud = srv_rel_obj.response.object;
00107 got_object_ = true;
00108 ROS_INFO ("Service call /get_released_object successful");
00109 }
00110 else
00111 {
00112 ROS_ERROR ("Failed to call service /get_released_object");
00113 }
00114 }
00115
00116
00118
00119 bool calculate_transform(kinect_cleanup::ReleaseObject::Request &req,
00120 kinect_cleanup::ReleaseObject::Response &res)
00121 {
00122 ros::Time time = ros::Time::now();
00123 bool found_transform = tf_.waitForTransform(world_, object_frame_,
00124 time, ros::Duration(tf_buffer_time_));
00125 if (found_transform)
00126 {
00127 tf::StampedTransform transform;
00128
00129 tf_.lookupTransform(world_, object_frame_, time, transform);
00130 sensor_msgs::PointCloud2 input_cloud;
00131 get_object(input_cloud);
00132 pcl_ros::transformPointCloud(world_, transform, input_cloud, output_cloud_);
00133
00134 }
00135 res.error = "OK";
00136 return true;
00137 }
00138
00139
00140
00141 void spin ()
00142 {
00143 ros::Rate loop_rate(20);
00144 while (ros::ok())
00145 {
00146 if (got_object_)
00147 {
00148 output_cloud_.header.stamp = ros::Time::now();
00149 pub_.publish (output_cloud_);
00150 ROS_INFO("[ObjectReleaser:] Point cloud published in frame %s", output_cloud_.header.frame_id.c_str());
00151
00152
00153 vision_msgs::cop_answer msg;
00154 vision_msgs::cop_descriptor cop_descriptor;
00155 vision_msgs::aposteriori_position aposteriori_position;
00156 msg.found_poses.push_back(aposteriori_position);
00157 msg.found_poses[0].models.push_back(cop_descriptor);
00158
00159 msg.found_poses[0].models[0].type = "ODUFinder";
00160
00161 msg.found_poses[0].models[0].sem_class = "mopfte20pmi";
00162 msg.found_poses[0].models[0].object_id = ++object_id_;
00163 msg.found_poses[0].position = 0;
00164 template_publisher_.publish(msg);
00165 }
00166 ros::spinOnce();
00167 loop_rate.sleep();
00168 }
00169 }
00170 };
00171
00172 int main (int argc, char** argv)
00173 {
00174 ros::init (argc, argv, "object_releaser_node");
00175 ros::NodeHandle n("~");
00176 ObjectReleaser object_releaser(n);
00177
00178 object_releaser.spin();
00179 return (0);
00180 }
00181
00182