point_cloud_config_marker.cpp
Go to the documentation of this file.
1 #include <iostream>
5 
6 using namespace std;
7 
8 visualization_msgs::Marker PointCloudConfigMarker::makeBoxMarker(geometry_msgs::Vector3 size){
9  visualization_msgs::Marker marker;
10  marker.type = visualization_msgs::Marker::CUBE;
11  marker.scale = size;
12 
13  marker.color.r = 0.5;
14  marker.color.g = 0.5;
15  marker.color.b = 0.5;
16  marker.color.a = 0.3;
17  return marker;
18 }
19 
20 visualization_msgs::Marker PointCloudConfigMarker::makeTextMarker(geometry_msgs::Vector3 size){
21  visualization_msgs::Marker marker;
22  marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
23  marker.text = "hello world";
24  marker.scale.z = 0.1;
25  marker.scale.y = 0.1;
26  marker.scale.x = 0.1;
27  marker.color.r = 0.0;
28  marker.color.g = 0.0;
29  marker.color.b = 0.0;
30  marker.color.a = 1.0;
31  return marker;
32 }
33 
34 visualization_msgs::InteractiveMarker PointCloudConfigMarker::makeBoxInteractiveMarker(MarkerControlConfig mconfig, std::string name){
35  visualization_msgs::InteractiveMarker mk;
36  mk.header.frame_id = base_frame;
37  if (latest_feedback_) {
38  mk.pose = latest_feedback_->pose;
39  }
40  //mk.pose =
41  std::string description;
42  std::stringstream ss;
43  ss << "size: (" << mconfig.size.x
44  << ", " << mconfig.size.y
45  << ", " << mconfig.size.z << ")" << std::endl;
46  ss << "resolution: " << mconfig.resolution_ << std::endl;
47  description = ss.str();
48 
49  mk.header.stamp = ros::Time(0);
50  mk.name = name;
51  //mk.scale = size * 1.05;
52 
53  mk.scale = 1.0;
54  mk.scale = max(mconfig.size.x, max(mconfig.size.y, mconfig.size.z));
55 
56  visualization_msgs::InteractiveMarkerControl controlBox;
57  controlBox.always_visible = true;
58  controlBox.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
59  controlBox.markers.push_back( makeBoxMarker(mconfig.size) );
60  mk.controls.push_back( controlBox );
61 
62  visualization_msgs::InteractiveMarkerControl textBox;
63  textBox.always_visible = true;
64  textBox.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
65  visualization_msgs::Marker text_marker = makeTextMarker(mconfig.size);
66  text_marker.text = description;
67  textBox.markers.push_back( text_marker );
68  mk.controls.push_back( textBox );
69 
70  visualization_msgs::InteractiveMarkerControl controlArrow;
71  controlArrow.always_visible = true;
72  controlArrow.orientation.w = 1;
73  controlArrow.orientation.x = 1;
74  controlArrow.orientation.y = 0;
75  controlArrow.orientation.z = 0;
76 
77  controlArrow.name = "move_x";
78  controlArrow.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
79  mk.controls.push_back(controlArrow);
80 
81  controlArrow.orientation.w = 1;
82  controlArrow.orientation.x = 0;
83  controlArrow.orientation.y = 1;
84  controlArrow.orientation.z = 0;
85  controlArrow.name = "move_z";
86  controlArrow.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
87  mk.controls.push_back(controlArrow);
88 
89  controlArrow.orientation.w = 1;
90  controlArrow.orientation.x = 0;
91  controlArrow.orientation.y = 0;
92  controlArrow.orientation.z = 1;
93  controlArrow.name = "move_y";
94  controlArrow.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
95  mk.controls.push_back(controlArrow);
96 
97  return mk;
98 }
99 
102  mh.insert("Add Point Cloud ROI", boost::bind( &PointCloudConfigMarker::publishMarkerMsg, this, _1));
103 
104  mh.insert("Start", boost::bind( &PointCloudConfigMarker::publishMarkerMsg, this, _1));
105 
106  mh.insert("Stop", boost::bind( &PointCloudConfigMarker::publishMarkerMsg, this, _1));
107 
108 
109 
110  resolution_menu_ = mh.insert( "Resolution" );
111  resolution_20cm_menu_ = mh.insert( resolution_menu_, "20cm", boost::bind( &PointCloudConfigMarker::changeResolutionCb, this, _1));
112  mh.setCheckState( resolution_20cm_menu_, interactive_markers::MenuHandler::UNCHECKED );
113 
114 
115  resolution_10cm_menu_ = mh.insert( resolution_menu_, "10cm", boost::bind( &PointCloudConfigMarker::changeResolutionCb, this, _1));
116  mh.setCheckState( resolution_10cm_menu_, interactive_markers::MenuHandler::UNCHECKED );
117 
118 
119  resolution_5cm_menu_ = mh.insert( resolution_menu_, "5cm", boost::bind( &PointCloudConfigMarker::changeResolutionCb, this, _1));
120  mh.setCheckState( resolution_5cm_menu_, interactive_markers::MenuHandler::CHECKED );
121  marker_control_config.resolution_ = 0.05;
122  checked_resolution_menu_ = resolution_5cm_menu_;
123  //box size menu
124  box_size_menu_ = mh.insert( "Box Size" );
125  // 100x100x100
126  box_size_100_menu_ = mh.insert( box_size_menu_, "100 x 100 x 100", boost::bind( &PointCloudConfigMarker::changeBoxSizeCb, this, _1));
128 
129  // 50x50x50
130  box_size_50_menu_ = mh.insert( box_size_menu_, "50 x 50 x 50", boost::bind( &PointCloudConfigMarker::changeBoxSizeCb, this, _1));
132 
133  checked_box_size_menu_ = box_size_50_menu_;
134 
135  // 25x25x25
136  box_size_25_menu_ = mh.insert( box_size_menu_, "25 x 25 x 25", boost::bind( &PointCloudConfigMarker::changeBoxSizeCb, this, _1));
138 
139  mh.insert("Cancel", boost::bind( &PointCloudConfigMarker::cancelCb, this, _1));
140  mh.insert("Clear All Point Cloud", boost::bind( &PointCloudConfigMarker::clearCb, this, _1));
141  /*
142  resolution_1cm_menu_ = mh.insert( resolution_menu_, "10 cm", boost::bind( &PointCloudConfigMarker::changeResolutionCb, this, _1));
143  mh.setCheckState( resolution_10cm_menu_, interactive_markers::MenuHandler::CHECKED );
144  */
145  return mh;
146 }
147 
148 void PointCloudConfigMarker::publishCurrentPose(const geometry_msgs::PoseStamped::ConstPtr &pose)
149 {
150  current_pose_pub_.publish(pose);
151 }
152 
153 void PointCloudConfigMarker::publishCurrentPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
154 {
155  geometry_msgs::PoseStamped pose_stamped;
156  pose_stamped.header = feedback->header;
157  pose_stamped.pose = feedback->pose;
158  current_pose_pub_.publish(pose_stamped);
159 }
160 
161 void PointCloudConfigMarker::moveBoxCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
162 {
163  //std::cout << "moved" << std::endl;
164  publishCurrentPose(feedback);
165  latest_feedback_ = feedback;
166 }
167 
168 void PointCloudConfigMarker::changeResolutionCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
169  menu_handler.setCheckState( checked_resolution_menu_ , interactive_markers::MenuHandler::UNCHECKED );
170  checked_resolution_menu_ = feedback->menu_entry_id;
171 
172  if(feedback->menu_entry_id == resolution_10cm_menu_){
173  marker_control_config.resolution_ = 0.1;
174 
175  }else if(feedback->menu_entry_id == resolution_5cm_menu_){
176  marker_control_config.resolution_ = 0.05;
177  }
178  else if(feedback->menu_entry_id == resolution_20cm_menu_){
179  marker_control_config.resolution_ = 0.2;
180  }
181  std::cout <<"resolution:" << marker_control_config.resolution_ << std::endl;
182 
183  menu_handler.setCheckState( feedback->menu_entry_id , interactive_markers::MenuHandler::CHECKED );
184 
185  menu_handler.reApply( *server_ );
186  server_->applyChanges();
187  latest_feedback_ = feedback;
188 
189 }
190 
191 
192 void PointCloudConfigMarker::changeBoxResolution(const std_msgs::Float32::ConstPtr &msg){
193  marker_control_config.resolution_ = msg->data;
194  updateBoxInteractiveMarker();
195 }
196 
197 void PointCloudConfigMarker::changeBoxSize(geometry_msgs::Vector3 size){
198  marker_control_config.size = size;
199  updateBoxInteractiveMarker();
200 }
201 
202 
203 void PointCloudConfigMarker::changeBoxSizeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
204  menu_handler.setCheckState( checked_resolution_menu_ , interactive_markers::MenuHandler::UNCHECKED );
205  checked_resolution_menu_ = feedback->menu_entry_id;
206 
207  menu_handler.setCheckState( feedback->menu_entry_id , interactive_markers::MenuHandler::CHECKED );
208 
209  geometry_msgs::Vector3 vec;
210  if(feedback->menu_entry_id == box_size_100_menu_){
211  vec.x = 1.0;
212  vec.y = 1.0;
213  vec.z = 1.0;
214  changeBoxSize(vec);
215  }else if(feedback->menu_entry_id == box_size_50_menu_){
216 
217  }
218 }
219 
220 
221 
222 
223 visualization_msgs::Marker PointCloudConfigMarker::makeMarkerMsg( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
224  visualization_msgs::Marker marker;
225  marker.type = visualization_msgs::Marker::CUBE;
226  marker.pose = feedback->pose;
227  marker.scale.x = marker_control_config.size.x;
228  marker.scale.y = marker_control_config.size.y;
229  marker.scale.z = marker_control_config.size.z;
230  marker.color.r = marker_control_config.resolution_;
231  marker.color.g = marker_control_config.resolution_ * 10;
232  if(marker.color.g > 1.0){marker.color.g = 1.0;}
233  marker.color.b = marker_control_config.resolution_ * 10;
234  if(marker.color.b > 1.0){marker.color.b = 1.0;}
235  marker.color.a = 0.1;
236  marker.header = feedback->header;
237  return marker;
238 }
239 
240 void PointCloudConfigMarker::publishMarkerMsg( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
241  visualization_msgs::Marker marker = makeMarkerMsg(feedback);
242  marker.id = marker_control_config.marker_id++;
243  pub_.publish(marker);
244  latest_feedback_ = feedback;
245 }
246 
247 
248 void PointCloudConfigMarker::cancelCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
249  if(marker_control_config.marker_id == 0){
250  return;
251  }
252  visualization_msgs::Marker marker = makeMarkerMsg(feedback);
253  marker.id = --marker_control_config.marker_id;
254  marker.action = visualization_msgs::Marker::DELETE;
255  pub_.publish(marker);
256  latest_feedback_ = feedback;
257 }
258 
259 
261  marker_control_config.marker_id = 0;
262 
263  visualization_msgs::Marker marker = makeMarkerMsg(latest_feedback_);
264  marker.id = -1;
265  marker.action = visualization_msgs::Marker::DELETE;
266  pub_.publish(marker);
267 }
268 
269 void PointCloudConfigMarker::clearBoxCB( const std_msgs::Empty::ConstPtr &msg) {
270  clearBox();
271 }
272 
273 void PointCloudConfigMarker::clearCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback){
274  latest_feedback_ = feedback;
275  clearBox();
276 }
277 
279  visualization_msgs::InteractiveMarker boxIM = makeBoxInteractiveMarker(marker_control_config, marker_name);
280 
281  server_->insert(boxIM,
282  boost::bind( &PointCloudConfigMarker::moveBoxCb, this, _1 ));
283  menu_handler.apply(*server_, marker_name);
284  server_->applyChanges();
285 }
286 
287 void PointCloudConfigMarker::updatePoseCB(const geometry_msgs::PoseStamped::ConstPtr &pose) {
288  //ROS_INFO("get new pose");
289  server_->setPose(marker_name, pose->pose);
290  server_->applyChanges();
291  publishCurrentPose(pose);
292  // manually update the latest_feedback_
293  visualization_msgs::InteractiveMarkerFeedback::Ptr feedback(new visualization_msgs::InteractiveMarkerFeedback);
294  feedback->pose = pose->pose;
295  feedback->header = pose->header;
296  latest_feedback_ = feedback;
297 }
298 
299 void PointCloudConfigMarker::addBoxCB(const std_msgs::Empty::ConstPtr &msg) {
300  ROS_INFO("add!");
301  visualization_msgs::Marker marker = makeMarkerMsg(latest_feedback_);
302  marker.id = marker_control_config.marker_id++;
303  pub_.publish(marker);
304 }
305 
307  pnh_.param("server_name", server_name, std::string ("") );
308  pnh_.param("size", size_, 1.0 );
309  pnh_.param("marker_name", marker_name, std::string ("point_cloud_config_marker") );
310  pnh_.param("base_frame", base_frame, std::string("/pelvis") );
311 
312  if ( server_name == "" ) {
314  }
316 
317  pub_ = pnh_.advertise<visualization_msgs::Marker> ("get", 1);
318  current_pose_pub_ = pnh_.advertise<geometry_msgs::PoseStamped> ("current_pose", 1);
320  this);
326 
328 
330 
331 
332  /*
333  updateBoxInter_markers::MenuHandler::EntryHandle sub_menu_move_;
334  sub_menu_move_ = model_menu_.insert( "Move" );
335  model_menu_.insert( sub_menu_move_, "Yes",
336  boost::bind( &PointCloudConfigMarker::jointMoveCB, this, _1) );
337  // model_menu_.insert( "Move" ,
338  //boost::bind( &PointCloudConfigMarker::jointMoveCB, this, _1) );
339  model_menu_.insert( "Reset Marker Pose",
340  boost::bind( &PointCloudConfigMarker::resetMarkerCB, this, _1) );
341  */
342 
343  return;
344 
345 }
346 
347 
348 int main(int argc, char** argv)
349 {
350  ros::init(argc, argv, "point_cloud_config_marker");
352  ros::spin();
353  return 0;
354 }
355 
356 
visualization_msgs::Marker makeBoxMarker(geometry_msgs::Vector3 size)
MarkerControlConfig marker_control_config
void updatePoseCB(const geometry_msgs::PoseStamped::ConstPtr &pose)
interactive_markers::MenuHandler makeMenuHandler()
visualization_msgs::InteractiveMarker makeBoxInteractiveMarker(MarkerControlConfig mconfig, std::string name)
void clearCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void changeBoxSizeCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void changeBoxResolution(const std_msgs::Float32::ConstPtr &msg)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ROSCPP_DECL const std::string & getName()
void moveBoxCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ros::Subscriber change_box_resolution_sub_
visualization_msgs::Marker makeMarkerMsg(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
std::shared_ptr< interactive_markers::InteractiveMarkerServer > server_
void publishMarkerMsg(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
ROSCPP_DECL void spin(Spinner &spinner)
void addBoxCB(const std_msgs::Empty::ConstPtr &msg)
bool setCheckState(EntryHandle handle, CheckState check_state)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int main(int argc, char **argv)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publishCurrentPose(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
void clearBoxCB(const std_msgs::Empty::ConstPtr &msg)
EntryHandle insert(const std::string &title, const FeedbackCallback &feedback_cb)
void cancelCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
visualization_msgs::Marker makeTextMarker(geometry_msgs::Vector3 size)
double max(double a, double b)
void changeBoxSize(geometry_msgs::Vector3 size)
interactive_markers::MenuHandler menu_handler
void changeResolutionCb(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)


jsk_interactive_marker
Author(s): furuta
autogenerated on Sat Mar 20 2021 03:03:33