interactive_point_cloud.h
Go to the documentation of this file.
1 #ifndef INTERACTIVE_POINT_CLOUD
2 #define INTERACTIVE_POINT_CLOUD
3 
4 #include <ros/ros.h>
5 
8 #include <sensor_msgs/PointCloud2.h>
9 #include <jsk_recognition_msgs/BoundingBoxArray.h>
10 #include <jsk_recognition_msgs/BoundingBoxMovement.h>
11 #include <jsk_recognition_msgs/Int32Stamped.h>
12 #include <geometry_msgs/PoseArray.h>
13 
15 //#include <point_cloud_server/StoreCloudAction.h>
16 #include <pcl/search/kdtree.h>
17 #include <tf/tf.h>
18 #include <tf/transform_listener.h>
19 
20 //#include <object_manipulator/tools/mechanism_interface.h>
21 #include "jsk_interactive_marker/InteractivePointCloudConfig.h"
23 #include <dynamic_reconfigure/server.h>
24 
29 
30 typedef pcl::PointXYZRGB PointT;
31 
32 
34 {
35 public:
36 
38  std::string marker_name,
39  std::string topic_name, std::string server_name);
40 
42 
44  void hide(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
45  void handlePoseCallback(const geometry_msgs::PoseStampedConstPtr &ps);
46  void pointCloudAndBoundingBoxCallback(const sensor_msgs::PointCloud2ConstPtr &cloud, const jsk_recognition_msgs::BoundingBoxArrayConstPtr &box, const geometry_msgs::PoseStampedConstPtr &handle);
47  void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud);
48  // void handlePoseAndBoundingBoxCallback(const geometry_msgs::PoseStampedConstPtr &ps, const jsk_recognition_msgs::BoundingBoxArrayConstPtr &box);
49  void setHandlePoseCallback(const geometry_msgs::PoseStampedConstPtr &ps);
50  void handlePoseAndBoundingBoxCallback(const jsk_recognition_msgs::Int32StampedConstPtr &index, const geometry_msgs::PoseArrayConstPtr &pa, const jsk_recognition_msgs::BoundingBoxArrayConstPtr &box);
51 private:
52 
53  typedef jsk_interactive_marker::InteractivePointCloudConfig Config;
54  std::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
56  virtual void configCallback (Config &config, uint32_t level);
57 
60  //typedef message_filters::sync_policies::ExactTime<geometry_msgs::PoseStamped, jsk_recognition_msgs::BoundingBoxArray> SyncHandlePose;
61 
63 
64 
65  void makeMenu();
66 
67  void pickup( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
68 
69  void makeMarker(const sensor_msgs::PointCloud2ConstPtr cloud, float size);
70  void makeMarker(const sensor_msgs::PointCloud2ConstPtr cloud, const jsk_recognition_msgs::BoundingBoxArrayConstPtr box, const geometry_msgs::PoseStampedConstPtr handle, float size);
71 
72  void menuPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
73  void move( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
74 
75  void leftClickPoint( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback );
76 
77  void markerFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback);
78 
79  void publishGraspPose();
80  void publishHandPose( geometry_msgs::PoseStamped box_pose);
81 
82  void setMarkerPoseCallback( const geometry_msgs::PoseStampedConstPtr &pose_stamped_msg);
83 
84  std::string marker_name_, topic_;
90  //ros::Subscriber sub_point_cloud_, sub_bounding_box_;
94  //message_filters::Subscriber<geometry_msgs::PoseStamped> sub_handle_pose_;
98  std::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
99  std::shared_ptr<message_filters::Synchronizer<SyncHandlePose> > sync_handle_;
100 
103 
104  sensor_msgs::PointCloud2 msg_cloud_;
105  double point_size_;
107 
108  geometry_msgs::PoseStamped marker_pose_;
110 
111  sensor_msgs::PointCloud2 current_croud_;
112  jsk_recognition_msgs::BoundingBoxArray current_box_;
113  geometry_msgs::PoseStamped handle_pose_;
117 
118  jsk_recognition_msgs::BoundingBoxMovement box_movement_;
119 };
120 
121 #endif
InteractivePointCloud::MenuHandler
interactive_markers::MenuHandler MenuHandler
Definition: interactive_point_cloud.h:58
InteractivePointCloud::setHandlePoseCallback
void setHandlePoseCallback(const geometry_msgs::PoseStampedConstPtr &ps)
Definition: interactive_point_cloud.cpp:108
InteractivePointCloud::pub_handle_pose_
ros::Publisher pub_handle_pose_
Definition: interactive_point_cloud.h:87
InteractivePointCloud::sub_point_cloud_
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_point_cloud_
Definition: interactive_point_cloud.h:91
InteractivePointCloud::pub_marker_pose_
ros::Publisher pub_marker_pose_
Definition: interactive_point_cloud.h:87
ros::Publisher
InteractivePointCloud::makeMenu
void makeMenu()
Definition: interactive_point_cloud.cpp:146
InteractivePointCloud::marker_server_
jsk_interactive_marker::ParentAndChildInteractiveMarkerServer marker_server_
Definition: interactive_point_cloud.h:101
InteractivePointCloud::input_pointcloud_
std::string input_pointcloud_
Definition: interactive_point_cloud.h:109
InteractivePointCloud::sub_handle_array_
message_filters::Subscriber< geometry_msgs::PoseArray > sub_handle_array_
Definition: interactive_point_cloud.h:96
InteractivePointCloud::handle_tf_
tf::Transform handle_tf_
Definition: interactive_point_cloud.h:114
InteractivePointCloud::SyncPolicy
message_filters::sync_policies::ExactTime< sensor_msgs::PointCloud2, jsk_recognition_msgs::BoundingBoxArray, geometry_msgs::PoseStamped > SyncPolicy
Definition: interactive_point_cloud.h:59
ros.h
InteractivePointCloud::setMarkerPoseCallback
void setMarkerPoseCallback(const geometry_msgs::PoseStampedConstPtr &pose_stamped_msg)
Definition: interactive_point_cloud.cpp:140
InteractivePointCloud::box_movement_
jsk_recognition_msgs::BoundingBoxMovement box_movement_
Definition: interactive_point_cloud.h:118
InteractivePointCloud::publishGraspPose
void publishGraspPose()
Definition: interactive_point_cloud.cpp:200
InteractivePointCloud::topic_
std::string topic_
Definition: interactive_point_cloud.h:84
InteractivePointCloud::Config
jsk_interactive_marker::InteractivePointCloudConfig Config
Definition: interactive_point_cloud.h:53
InteractivePointCloud::InteractivePointCloud
InteractivePointCloud(std::string marker_name, std::string topic_name, std::string server_name)
Definition: interactive_point_cloud.cpp:20
menu_handler.h
InteractivePointCloud::menuPoint
void menuPoint(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
time_synchronizer.h
InteractivePointCloud::hide
void hide(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Clear the cloud stored in this object.
Definition: interactive_point_cloud.cpp:185
InteractivePointCloud::sub_handle_pose_
ros::Subscriber sub_handle_pose_
Definition: interactive_point_cloud.h:88
InteractivePointCloud::pub_handle_pose_array_
ros::Publisher pub_handle_pose_array_
Definition: interactive_point_cloud.h:87
InteractivePointCloud::msg_cloud_
sensor_msgs::PointCloud2 msg_cloud_
Definition: interactive_point_cloud.h:104
message_filters::Subscriber< sensor_msgs::PointCloud2 >
InteractivePointCloud::initial_handle_pose_
std::string initial_handle_pose_
Definition: interactive_point_cloud.h:109
InteractivePointCloud::handle_pose_
geometry_msgs::PoseStamped handle_pose_
Definition: interactive_point_cloud.h:113
InteractivePointCloud::display_interactive_manipulator_
bool display_interactive_manipulator_
Definition: interactive_point_cloud.h:116
InteractivePointCloud::sub_marker_pose_
ros::Subscriber sub_marker_pose_
Definition: interactive_point_cloud.h:89
InteractivePointCloud::markerFeedback
void markerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_point_cloud.cpp:192
InteractivePointCloud::point_size_
double point_size_
Definition: interactive_point_cloud.h:105
interactive_markers::MenuHandler
mutex
boost::mutex mutex
Definition: bounding_box_marker.cpp:44
InteractivePointCloud::nh_
ros::NodeHandle nh_
Definition: interactive_point_cloud.h:85
InteractivePointCloud::srv_
std::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
Definition: interactive_point_cloud.h:54
simple_action_client.h
InteractivePointCloud::pub_left_click_
ros::Publisher pub_left_click_
Definition: interactive_point_cloud.h:87
InteractivePointCloud::exist_handle_tf_
bool exist_handle_tf_
Definition: interactive_point_cloud.h:115
subscriber.h
PointT
pcl::PointXYZRGB PointT
Definition: interactive_point_cloud.h:30
InteractivePointCloud::handlePoseCallback
void handlePoseCallback(const geometry_msgs::PoseStampedConstPtr &ps)
InteractivePointCloud::pointCloudAndBoundingBoxCallback
void pointCloudAndBoundingBoxCallback(const sensor_msgs::PointCloud2ConstPtr &cloud, const jsk_recognition_msgs::BoundingBoxArrayConstPtr &box, const geometry_msgs::PoseStampedConstPtr &handle)
Definition: interactive_point_cloud.cpp:102
InteractivePointCloud::use_bounding_box_
bool use_bounding_box_
Definition: interactive_point_cloud.h:106
InteractivePointCloud::makeMarker
void makeMarker(const sensor_msgs::PointCloud2ConstPtr cloud, float size)
Definition: interactive_point_cloud.cpp:228
parent_and_child_interactive_marker_server.h
InteractivePointCloud::marker_pose_
geometry_msgs::PoseStamped marker_pose_
Definition: interactive_point_cloud.h:108
InteractivePointCloud::pickup
void pickup(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
jsk_interactive_marker::ParentAndChildInteractiveMarkerServer
Definition: parent_and_child_interactive_marker_server.h:31
InteractivePointCloud::sync_
std::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
Definition: interactive_point_cloud.h:98
InteractivePointCloud::leftClickPoint
void leftClickPoint(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_point_cloud.cpp:160
tf::Transform
InteractivePointCloud::pub_click_point_
ros::Publisher pub_click_point_
Definition: interactive_point_cloud.h:87
InteractivePointCloud::move
void move(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Definition: interactive_point_cloud.cpp:154
InteractivePointCloud::mutex_
boost::mutex mutex_
Definition: interactive_point_cloud.h:55
InteractivePointCloud::pub_left_click_relative_
ros::Publisher pub_left_click_relative_
Definition: interactive_point_cloud.h:87
InteractivePointCloud::sync_handle_
std::shared_ptr< message_filters::Synchronizer< SyncHandlePose > > sync_handle_
Definition: interactive_point_cloud.h:99
transform_listener.h
tf.h
InteractivePointCloud::marker_name_
std::string marker_name_
Definition: interactive_point_cloud.h:84
InteractivePointCloud::sub_initial_handle_pose_
message_filters::Subscriber< geometry_msgs::PoseStamped > sub_initial_handle_pose_
Definition: interactive_point_cloud.h:93
interactive_marker_server.h
InteractivePointCloud::sub_selected_index_
message_filters::Subscriber< jsk_recognition_msgs::Int32Stamped > sub_selected_index_
Definition: interactive_point_cloud.h:95
InteractivePointCloud::pub_box_movement_
ros::Publisher pub_box_movement_
Definition: interactive_point_cloud.h:87
tf::TransformListener
index
char * index(char *sp, char c)
InteractivePointCloud::menu_handler_
interactive_markers::MenuHandler menu_handler_
Definition: interactive_point_cloud.h:102
synchronizer.h
InteractivePointCloud::input_bounding_box_
std::string input_bounding_box_
Definition: interactive_point_cloud.h:109
approximate_time.h
InteractivePointCloud::~InteractivePointCloud
~InteractivePointCloud()
Definition: interactive_point_cloud.cpp:71
InteractivePointCloud::SyncHandlePose
message_filters::sync_policies::ExactTime< jsk_recognition_msgs::Int32Stamped, geometry_msgs::PoseArray, jsk_recognition_msgs::BoundingBoxArray > SyncHandlePose
Definition: interactive_point_cloud.h:62
InteractivePointCloud::pointCloudCallback
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr &cloud)
Definition: interactive_point_cloud.cpp:98
message_filters::sync_policies::ExactTime
InteractivePointCloud::tfl_
tf::TransformListener tfl_
Definition: interactive_point_cloud.h:97
InteractivePointCloud::sub_bounding_box_
message_filters::Subscriber< jsk_recognition_msgs::BoundingBoxArray > sub_bounding_box_
Definition: interactive_point_cloud.h:92
InteractivePointCloud::current_croud_
sensor_msgs::PointCloud2 current_croud_
Definition: interactive_point_cloud.h:111
InteractivePointCloud
Definition: interactive_point_cloud.h:33
InteractivePointCloud::pub_grasp_pose_
ros::Publisher pub_grasp_pose_
Definition: interactive_point_cloud.h:87
InteractivePointCloud::publishHandPose
void publishHandPose(geometry_msgs::PoseStamped box_pose)
Definition: interactive_point_cloud.cpp:204
InteractivePointCloud::configCallback
virtual void configCallback(Config &config, uint32_t level)
Definition: interactive_point_cloud.cpp:73
InteractivePointCloud::current_box_
jsk_recognition_msgs::BoundingBoxArray current_box_
Definition: interactive_point_cloud.h:112
ros::NodeHandle
ros::Subscriber
InteractivePointCloud::handlePoseAndBoundingBoxCallback
void handlePoseAndBoundingBoxCallback(const jsk_recognition_msgs::Int32StampedConstPtr &index, const geometry_msgs::PoseArrayConstPtr &pa, const jsk_recognition_msgs::BoundingBoxArrayConstPtr &box)
InteractivePointCloud::pnh_
ros::NodeHandle pnh_
Definition: interactive_point_cloud.h:86


jsk_interactive_marker
Author(s): furuta
autogenerated on Fri Aug 2 2024 08:50:24