interactive_point_cloud.cpp
Go to the documentation of this file.
00001 #include <visualization_msgs/InteractiveMarker.h>
00002 #include <visualization_msgs/InteractiveMarkerFeedback.h>
00003 #include <geometry_msgs/PoseStamped.h>
00004 #include <std_msgs/String.h>
00005 #include <tf/tf.h>
00006 
00007 #include <jsk_interactive_marker/interactive_marker_helpers.h>
00008 #include <jsk_interactive_marker/interactive_point_cloud.h>
00009 #include <tf/transform_datatypes.h>
00010 #include <pcl_ros/transforms.h>
00011 #include <tf/transform_listener.h>
00012 #include <pcl_conversions/pcl_conversions.h>
00013 
00014 #include <boost/range/algorithm_ext/erase.hpp>
00015 
00016 using namespace visualization_msgs;
00017 using namespace interactive_markers;
00018 using namespace im_helpers;
00019 
00020 InteractivePointCloud::InteractivePointCloud(std::string marker_name,
00021                            std::string topic_name, std::string server_name ):
00022   marker_name_(marker_name)
00023   , nh_()
00024   , pnh_("~")
00025   , tfl_(nh_)
00026   , marker_server_(topic_name, server_name, false)
00027 {
00028 
00029   pnh_.param<double>("point_size", point_size_, 0.004);
00030   pnh_.param<std::string>("input", input_pointcloud_, "/selected_pointcloud");
00031   //  pnh_.param<bool>("use_bounding_box", use_bounding_box_, "false");
00032   pnh_.param<bool>("use_bounding_box", use_bounding_box_, "true");
00033   pnh_.param<std::string>("input_bounding_box", input_bounding_box_, "/bounding_box_marker/selected_box_array");
00034   pnh_.param<std::string>("handle_pose", initial_handle_pose_, "/handle_estimator/output_best");
00035   pnh_.param<bool>("display_interactive_manipulator", display_interactive_manipulator_, true);
00036   //publish
00037   pub_click_point_ = pnh_.advertise<geometry_msgs::PointStamped>("right_click_point", 1);
00038   pub_left_click_ = pnh_.advertise<geometry_msgs::PointStamped>("left_click_point", 1);
00039   pub_left_click_relative_ = pnh_.advertise<geometry_msgs::PointStamped>("left_click_point_relative", 1);
00040   pub_marker_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("marker_pose", 1);
00041   pub_grasp_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("grasp_pose", 1);
00042   pub_box_movement_ = pnh_.advertise<jsk_recognition_msgs::BoundingBoxMovement>("box_movement", 1);
00043   pub_handle_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("handle_pose", 1);
00044   pub_handle_pose_array_ = pnh_.advertise<geometry_msgs::PoseArray>("handle_pose_array", 1);
00045   
00046   //subscribe
00047   sub_handle_pose_ = pnh_.subscribe<geometry_msgs::PoseStamped> ("set_handle_pose", 1, boost::bind( &InteractivePointCloud::setHandlePoseCallback, this, _1));
00048   sub_marker_pose_ = pnh_.subscribe<geometry_msgs::PoseStamped>("set_marker_pose", 1, boost::bind( &InteractivePointCloud::setMarkerPoseCallback, this, _1)); 
00049   sub_point_cloud_.subscribe(pnh_, input_pointcloud_, 1);
00050 
00051 
00052   if(use_bounding_box_){
00053     //point cloud and bounding box
00054     sync_ = std::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
00055     sub_bounding_box_.subscribe(pnh_,input_bounding_box_, 1);
00056     sub_initial_handle_pose_.subscribe(pnh_, initial_handle_pose_, 1);
00057     sync_->connectInput(sub_point_cloud_, sub_bounding_box_, sub_initial_handle_pose_);
00058     sync_->registerCallback(boost::bind(&InteractivePointCloud::pointCloudAndBoundingBoxCallback, this, _1, _2, _3));
00059 
00060   }else{
00061       sub_point_cloud_.registerCallback(&InteractivePointCloud::pointCloudCallback, this);
00062   }
00063   srv_ = std::make_shared <dynamic_reconfigure::Server<Config> > (pnh_);
00064   dynamic_reconfigure::Server<Config>::CallbackType f =
00065     boost::bind (&InteractivePointCloud::configCallback, this, _1, _2);
00066   srv_->setCallback (f);
00067 
00068   makeMenu();
00069 }
00070 
00071 InteractivePointCloud::~InteractivePointCloud(){};
00072 
00073 void InteractivePointCloud::configCallback(Config &config, uint32_t level)
00074 {
00075   boost::mutex::scoped_lock(mutex_);
00076   point_size_ = config.point_size;
00077   if(display_interactive_manipulator_ != config.display_interactive_manipulator){
00078     display_interactive_manipulator_ = config.display_interactive_manipulator;
00079     visualization_msgs::InteractiveMarker int_marker;
00080     marker_server_.get(marker_name_, int_marker );
00081     if(display_interactive_manipulator_){
00082       addVisible6DofControl(int_marker);
00083     }else{
00084       for(std::vector<visualization_msgs::InteractiveMarkerControl>::iterator it=int_marker.controls.begin(); it!=int_marker.controls.end();){  
00085         if(it->interaction_mode==visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS
00086            || it->interaction_mode==visualization_msgs::InteractiveMarkerControl::MOVE_AXIS){
00087           it = int_marker.controls.erase(it);
00088         }else{
00089           it++;
00090         }
00091       }
00092     }
00093     marker_server_.erase(marker_name_);
00094     marker_server_.insert(int_marker);
00095     marker_server_.applyChanges();
00096   }
00097 }
00098 
00099 void InteractivePointCloud::pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud){
00100   makeMarker(cloud, point_size_ );
00101 }
00102 
00103 void InteractivePointCloud::pointCloudAndBoundingBoxCallback(const sensor_msgs::PointCloud2ConstPtr &cloud, const jsk_recognition_msgs::BoundingBoxArrayConstPtr &box, const geometry_msgs::PoseStampedConstPtr &handle){
00104   makeMarker(cloud, box, handle, point_size_ );
00105 }
00106 
00107 
00108 
00109 void InteractivePointCloud::setHandlePoseCallback(const geometry_msgs::PoseStampedConstPtr &ps){
00110   if( ps->header.stamp == current_box_.header.stamp ){
00111     if(current_box_.boxes.size() > 0){
00112       handle_pose_ = *ps;
00113       
00114       tf::Transform tf_ps, tf_box;
00115       tf_ps.setOrigin(tf::Vector3(ps->pose.position.x, ps->pose.position.y, ps->pose.position.z));
00116       tf_ps.setRotation(tf::Quaternion( ps->pose.orientation.x, ps->pose.orientation.y, ps->pose.orientation.z, ps->pose.orientation.w));
00117 
00118       geometry_msgs::Pose pose = current_box_.boxes[0].pose;
00119       tf_box.setOrigin(tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
00120       tf_box.setRotation(tf::Quaternion( pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w));
00121 
00122       handle_tf_ = tf_box.inverseTimes(tf_ps);
00123       exist_handle_tf_ = true;
00124       publishHandPose( marker_pose_ );
00125 
00126       /* set box_movement_ */
00127       geometry_msgs::Pose handle_pose;
00128       handle_pose.position.x = handle_tf_.getOrigin().x();
00129       handle_pose.position.y = handle_tf_.getOrigin().y();
00130       handle_pose.position.z = handle_tf_.getOrigin().z();
00131       handle_pose.orientation.x = handle_tf_.getRotation().x();
00132       handle_pose.orientation.y = handle_tf_.getRotation().y();
00133       handle_pose.orientation.z = handle_tf_.getRotation().z();
00134       handle_pose.orientation.w = handle_tf_.getRotation().w();
00135 
00136       box_movement_.handle_pose = handle_pose;
00137     }
00138   }
00139 }
00140 
00141 void InteractivePointCloud::setMarkerPoseCallback( const geometry_msgs::PoseStampedConstPtr &pose_stamped_msg){
00142   marker_server_.setPose(marker_name_, pose_stamped_msg->pose, pose_stamped_msg->header);
00143   marker_server_.applyChanges();
00144 }
00145 
00146 // create menu
00147 void InteractivePointCloud::makeMenu()
00148 {
00149   menu_handler_.insert( "Move",  boost::bind( &InteractivePointCloud::move, this, _1) );
00150 
00151   menu_handler_.insert( "Hide",  boost::bind( &InteractivePointCloud::hide, this, _1));
00152 }
00153 
00154 //publish marker pose
00155 void InteractivePointCloud::move( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback ){
00156   box_movement_.destination = marker_pose_;
00157   pub_box_movement_.publish(box_movement_);
00158 
00159 }
00160 
00161 void InteractivePointCloud::leftClickPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00162 {
00163   if(!feedback->mouse_point_valid)
00164   {
00165     ROS_WARN("Clicked point had an invalid position. Not looking there!");
00166     return;
00167   }
00168 
00169   ROS_DEBUG_STREAM_NAMED("cloud_handler", "Button click in frame "
00170                          << feedback->header.frame_id << " at point\n" << feedback->mouse_point );
00171   geometry_msgs::PointStamped click_point;
00172   click_point.point = feedback->mouse_point;
00173   click_point.header = feedback->header;
00174   click_point.header.stamp = ros::Time::now();
00175   pub_left_click_.publish(click_point);
00176   tf::Transform transform;
00177   tf::poseMsgToTF(feedback->pose, transform);
00178   tf::Vector3 vector_absolute(feedback->mouse_point.x, feedback->mouse_point.y, feedback->mouse_point.z); 
00179   tf::Vector3 vector_relative=transform.inverse() * vector_absolute;
00180   click_point.point.x=vector_relative.getX();
00181   click_point.point.y=vector_relative.getY();   
00182   click_point.point.z=vector_relative.getZ();
00183   pub_left_click_relative_.publish(click_point);
00184 }
00185 
00186 void InteractivePointCloud::hide( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00187 {
00188   marker_server_.erase(marker_name_);
00189   marker_server_.applyChanges();
00190 }
00191 
00192 
00193 void InteractivePointCloud::markerFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
00194   marker_pose_.pose = feedback->pose;
00195   marker_pose_.header = feedback->header;
00196   if(exist_handle_tf_){
00197     publishHandPose( marker_pose_);
00198   }
00199 }
00200 
00201 void InteractivePointCloud::publishGraspPose(){
00202   pub_grasp_pose_.publish(handle_pose_);
00203 }
00204 
00205 void InteractivePointCloud::publishHandPose( geometry_msgs::PoseStamped box_pose){
00206   tf::Transform tf_marker;
00207   tf_marker.setOrigin(tf::Vector3(box_pose.pose.position.x, box_pose.pose.position.y, box_pose.pose.position.z));
00208   tf_marker.setRotation(tf::Quaternion( box_pose.pose.orientation.x, box_pose.pose.orientation.y, box_pose.pose.orientation.z, box_pose.pose.orientation.w));
00209 
00210   tf_marker = tf_marker * handle_tf_;
00211   geometry_msgs::PoseStamped handle_pose;
00212   handle_pose.header.frame_id = box_pose.header.frame_id;
00213   handle_pose.pose.position.x = tf_marker.getOrigin().x();
00214   handle_pose.pose.position.y = tf_marker.getOrigin().y();
00215   handle_pose.pose.position.z = tf_marker.getOrigin().z();
00216   handle_pose.pose.orientation.x = tf_marker.getRotation().x();
00217   handle_pose.pose.orientation.y = tf_marker.getRotation().y();
00218   handle_pose.pose.orientation.z = tf_marker.getRotation().z();
00219   handle_pose.pose.orientation.w = tf_marker.getRotation().w();
00220 
00221   pub_handle_pose_.publish(handle_pose);
00222 
00223   geometry_msgs::PoseArray handle_pose_array;
00224   handle_pose_array.header = handle_pose.header;
00225   handle_pose_array.poses.push_back(handle_pose.pose);
00226   pub_handle_pose_array_.publish(handle_pose_array);
00227 }
00228 
00229 void InteractivePointCloud::makeMarker(const sensor_msgs::PointCloud2ConstPtr cloud,float size){
00230   makeMarker(cloud, jsk_recognition_msgs::BoundingBoxArray::ConstPtr(), geometry_msgs::PoseStamped::ConstPtr(), size);
00231 }
00232 void InteractivePointCloud::makeMarker(const sensor_msgs::PointCloud2ConstPtr cloud, const jsk_recognition_msgs::BoundingBoxArrayConstPtr box, const geometry_msgs::PoseStampedConstPtr handle, float size)
00233 {
00234   exist_handle_tf_ = false;
00235   if(cloud){
00236     current_croud_ = *cloud;
00237   }
00238   if(box){
00239     current_box_ = *box;
00240   }
00241 
00242   InteractiveMarker int_marker;
00243   int_marker.name = marker_name_;
00244 
00245   pcl::PointCloud<PointT> pcl_cloud, transform_cloud;
00246   pcl::fromROSMsg(*cloud, pcl_cloud);
00247 
00248   if(box && box->boxes.size() > 0){
00249     jsk_recognition_msgs::BoundingBox first_box = box->boxes[0];
00250     int_marker.pose = first_box.pose;
00251     int_marker.header.frame_id = first_box.header.frame_id;
00252 
00253     marker_pose_.header = box->header;
00254     marker_pose_.pose = first_box.pose;
00255 
00256     tf::Transform transform;
00257     tf::poseMsgToTF(first_box.pose, transform);
00258 
00259     pcl_ros::transformPointCloud(pcl_cloud, transform_cloud, transform.inverse());
00260     pcl_cloud = transform_cloud;
00261 
00262     box_movement_.box = first_box;
00263     box_movement_.header.frame_id = first_box.header.frame_id;
00264 
00265     if(handle->header.frame_id != ""){
00266       setHandlePoseCallback(handle);
00267     }
00268   }
00269   else{
00270     int_marker.pose.position.x=int_marker.pose.position.y=int_marker.pose.position.z=int_marker.pose.orientation.x=int_marker.pose.orientation.y=int_marker.pose.orientation.z=0;
00271     int_marker.pose.orientation.w=1;
00272   }
00273 
00274 
00275   int num_points = pcl_cloud.points.size();
00276 
00277   Marker marker;
00278   marker.color.r = 1.0;
00279   marker.color.g = 1.0;
00280   marker.color.b = 1.0;
00281   marker.color.a = 1.0;
00282   marker.frame_locked = false;
00283   if(num_points > 0)
00284   {
00285 
00286       int_marker.header = cloud->header;
00287 
00288       InteractiveMarkerControl control;
00289       control.always_visible = true;
00290       control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00291       //control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_3D;
00292       control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
00293 
00294       int_marker.header.stamp = ros::Time::now();
00295 
00296       marker.scale.x = size;
00297       marker.scale.y = size;
00298       marker.scale.z = size;
00299       marker.type = visualization_msgs::Marker::SPHERE_LIST;
00300 
00301       marker.points.resize( num_points );
00302       marker.colors.resize( num_points );
00303       //point cloud
00304       for ( int i=0; i<num_points; i++)
00305         {
00306           marker.points[i].x = pcl_cloud.points[i].x;
00307           marker.points[i].y = pcl_cloud.points[i].y;
00308           marker.points[i].z = pcl_cloud.points[i].z;
00309           marker.colors[i].r = pcl_cloud.points[i].r/255.;
00310           marker.colors[i].g = pcl_cloud.points[i].g/255.;
00311           marker.colors[i].b = pcl_cloud.points[i].b/255.;
00312           marker.colors[i].a = 1.0;
00313         }
00314       control.markers.push_back( marker );
00315 
00316       //bounding box
00317       if(box && box->boxes.size() > 0){
00318         Marker bounding_box_marker;
00319         bounding_box_marker.color.r = 0.0;
00320         bounding_box_marker.color.g = 0.0;
00321         bounding_box_marker.color.b = 1.0;
00322         bounding_box_marker.color.a = 1.0;
00323         bounding_box_marker.type = visualization_msgs::Marker::LINE_LIST;
00324         bounding_box_marker.scale.x = 0.01; //line width
00325         bounding_box_marker.points.resize(24);
00326 
00327         double x = box->boxes[0].dimensions.x / 2;
00328         double y = box->boxes[0].dimensions.y / 2;
00329         double z = box->boxes[0].dimensions.z / 2;
00330         for(int i=0; i<4; i++)
00331           {
00332             if(i%2 == 0){
00333               bounding_box_marker.points[2*i].x = bounding_box_marker.points[2*i+1].x = x;
00334               
00335             }else{
00336               bounding_box_marker.points[2*i].x = bounding_box_marker.points[2*i+1].x = - x;
00337             }
00338             if(i%4 < 2){
00339               bounding_box_marker.points[2*i].y = bounding_box_marker.points[2*i+1].y = y;
00340               
00341             }else{
00342               bounding_box_marker.points[2*i].y = bounding_box_marker.points[2*i+1].y = - y;
00343               
00344             }
00345             bounding_box_marker.points[2*i].z = z;
00346             bounding_box_marker.points[2*i+1].z = - z;
00347           }
00348 
00349         for(int i=4; i<8; i++)
00350           {
00351             if(i%2 == 0){
00352               bounding_box_marker.points[2*i].x = bounding_box_marker.points[2*i+1].x = x;
00353               
00354             }else{
00355               bounding_box_marker.points[2*i].x = bounding_box_marker.points[2*i+1].x = - x;
00356             }
00357             if(i%4 < 2){
00358               bounding_box_marker.points[2*i].z = bounding_box_marker.points[2*i+1].z = z;
00359               
00360             }else{
00361               bounding_box_marker.points[2*i].z = bounding_box_marker.points[2*i+1].z = - z;
00362               
00363             }
00364             bounding_box_marker.points[2*i].y = y;
00365             bounding_box_marker.points[2*i+1].y = - y;
00366           }
00367 
00368         for(int i=8; i<12; i++)
00369           {
00370             if(i%2 == 0){
00371               bounding_box_marker.points[2*i].z = bounding_box_marker.points[2*i+1].z = z;
00372               
00373             }else{
00374               bounding_box_marker.points[2*i].z = bounding_box_marker.points[2*i+1].z = - z;
00375             }
00376             if(i%4 < 2){
00377               bounding_box_marker.points[2*i].y = bounding_box_marker.points[2*i+1].y = y;
00378               
00379             }else{
00380               bounding_box_marker.points[2*i].y = bounding_box_marker.points[2*i+1].y = - y;
00381               
00382             }
00383             bounding_box_marker.points[2*i].x = x;
00384             bounding_box_marker.points[2*i+1].x = - x;
00385           }
00386 
00387         control.markers.push_back( bounding_box_marker );
00388       }
00389       int_marker.controls.push_back( control );
00390       if(display_interactive_manipulator_){
00391         addVisible6DofControl(int_marker);
00392       }
00393       marker_server_.insert( int_marker, boost::bind( &InteractivePointCloud::leftClickPoint, this, _1 ),
00394                              visualization_msgs::InteractiveMarkerFeedback::BUTTON_CLICK);
00395       marker_server_.setCallback( int_marker.name,
00396                                   boost::bind( &InteractivePointCloud::markerFeedback, this, _1) );
00397 
00398       menu_handler_.apply( marker_server_, marker_name_ );
00399       marker_server_.applyChanges();
00400       ROS_INFO("made interactive point cloud");
00401       
00402       publishGraspPose();
00403   }
00404 }


jsk_interactive_marker
Author(s): furuta
autogenerated on Wed May 1 2019 02:40:31