transformable_interactive_server.cpp
Go to the documentation of this file.
3 
4 using namespace jsk_interactive_marker;
5 
7  n_->param("interactive_manipulator_orientation", interactive_manipulator_orientation_ , 0);
8  n_->param("torus_udiv", torus_udiv_, 20);
9  n_->param("torus_vdiv", torus_vdiv_, 20);
10  n_->param("strict_tf", strict_tf_, false);
12  setpose_sub_ = n_->subscribe<geometry_msgs::PoseStamped>("set_pose", 1, boost::bind(&TransformableInteractiveServer::setPose, this, _1, false));
13  setcontrolpose_sub_ = n_->subscribe<geometry_msgs::PoseStamped>("set_control_pose", 1, boost::bind(&TransformableInteractiveServer::setPose, this, _1, true));
15 
21 
24 
26 
28 
29  focus_name_text_pub_ = n_->advertise<jsk_rviz_plugins::OverlayText>("focus_marker_name_text", 1);
30  focus_pose_text_pub_ = n_->advertise<jsk_rviz_plugins::OverlayText>("focus_marker_pose_text", 1);
31  focus_object_marker_name_pub_ = n_->advertise<std_msgs::String>("focus_object_marker_name", 1);
32  pose_pub_ = n_->advertise<geometry_msgs::PoseStamped>("pose", 1);
33  pose_with_name_pub_ = n_->advertise<jsk_interactive_marker::PoseStampedWithName>("pose_with_name", 1);
34 
35  get_pose_srv_ = n_->advertiseService<jsk_interactive_marker::GetTransformableMarkerPose::Request, jsk_interactive_marker::GetTransformableMarkerPose::Response>("get_pose", boost::bind(&TransformableInteractiveServer::getPoseService, this, _1, _2, false));
36  get_control_pose_srv_ = n_->advertiseService<jsk_interactive_marker::GetTransformableMarkerPose::Request, jsk_interactive_marker::GetTransformableMarkerPose::Response>("get_control_pose", boost::bind(&TransformableInteractiveServer::getPoseService, this, _1, _2, true));
37  set_pose_srv_ = n_->advertiseService<jsk_interactive_marker::SetTransformableMarkerPose::Request ,jsk_interactive_marker::SetTransformableMarkerPose::Response>("set_pose", boost::bind(&TransformableInteractiveServer::setPoseService, this, _1, _2, false));
38  set_control_pose_srv_ = n_->advertiseService<jsk_interactive_marker::SetTransformableMarkerPose::Request ,jsk_interactive_marker::SetTransformableMarkerPose::Response>("set_control_pose", boost::bind(&TransformableInteractiveServer::setPoseService, this, _1, _2, true));
49  marker_dimensions_pub_ = n_->advertise<jsk_interactive_marker::MarkerDimensions>("marker_dimensions", 1);
51 
52  config_srv_ = std::make_shared <dynamic_reconfigure::Server<InteractiveSettingConfig> > (*n_);
53  dynamic_reconfigure::Server<InteractiveSettingConfig>::CallbackType f =
54  boost::bind (&TransformableInteractiveServer::configCallback, this, _1, _2);
55  config_srv_->setCallback (f);
56 
58 
59  // initialize yaml-menu-handler
60  std::string yaml_filename;
61  n_->param("yaml_filename", yaml_filename, std::string(""));
62  yaml_menu_handler_ptr_ = std::make_shared <YamlMenuHandler> (n_, yaml_filename);
63  yaml_menu_handler_ptr_->_menu_handler.insert(
64  "enable manipulator",
65  boost::bind(&TransformableInteractiveServer::enableInteractiveManipulatorDisplay, this, _1, /*enable=*/true));
66  yaml_menu_handler_ptr_->_menu_handler.insert(
67  "disable manipulator",
68  boost::bind(&TransformableInteractiveServer::enableInteractiveManipulatorDisplay, this, _1, /*enable=*/false));
69 
70  bool use_parent_and_child;
71  n_->param("use_parent_and_child", use_parent_and_child, false);
72  if (use_parent_and_child)
73  {
74  ROS_INFO("initialize parent and child marker");
76  }
77  else
78  {
79  ROS_INFO("initialize simple marker");
81  }
82 }
83 
85 {
86  for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin(); itpairstri != transformable_objects_map_.end(); itpairstri++) {
87  delete itpairstri->second;
88  }
90 }
91 
92 void TransformableInteractiveServer::configCallback(InteractiveSettingConfig &config, uint32_t level)
93  {
94  boost::mutex::scoped_lock lock(mutex_);
95  config_ = config;
96  int interaction_mode = config.interaction_mode;
97  for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin(); itpairstri != transformable_objects_map_.end(); itpairstri++) {
98  TransformableObject* tobject = itpairstri->second;
100  updateTransformableObject(tobject);
101  }
102  }
103 
104 
106  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
107 {
108  switch ( feedback->event_type )
109  {
110  case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
111  case visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT:
112  focus_object_marker_name_ = feedback->marker_name;
117  break;
118 
119  case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
120  TransformableObject* tobject = transformable_objects_map_[feedback->marker_name.c_str()];
121  if(tobject){
122  geometry_msgs::PoseStamped input_pose_stamped;
123  input_pose_stamped.header = feedback->header;
124  input_pose_stamped.pose = feedback->pose;
125  setPoseWithTfTransformation(tobject, input_pose_stamped, true);
126  }else{
127  ROS_ERROR("Invalid ObjectId Request Received %s", feedback->marker_name.c_str());
128  }
132  break;
133  }
134 }
135 
136 void TransformableInteractiveServer::setColor(std_msgs::ColorRGBA msg)
137 {
140  tobject->setRGBA(msg.r, msg.g, msg.b, msg.a);
141  updateTransformableObject(tobject);
142 }
143 
144 void TransformableInteractiveServer::setRadius(std_msgs::Float32 msg)
145 {
148  if(tobject->setRadius(msg)){
149  updateTransformableObject(tobject);
151  }
152 }
153 
155 {
158  if(tobject->setSmallRadius(msg)){
159  updateTransformableObject(tobject);
161  }
162 }
163 
164 void TransformableInteractiveServer::setX(std_msgs::Float32 msg)
165 {
168  if(tobject->setX(msg)){
169  updateTransformableObject(tobject);
171  }
172 }
173 
174 void TransformableInteractiveServer::setY(std_msgs::Float32 msg)
175 {
178  if(tobject->setY(msg)){
179  updateTransformableObject(tobject);
181  }
182 }
183 
184 void TransformableInteractiveServer::setZ(std_msgs::Float32 msg)
185 {
188  if(tobject->setZ(msg)){
189  updateTransformableObject(tobject);
191  }
192 }
193 
195 {
196  visualization_msgs::InteractiveMarker int_marker = tobject->getInteractiveMarker();
197  server_->insert(int_marker, boost::bind( &TransformableInteractiveServer::processFeedback,this, _1));
200 }
201 
202 void TransformableInteractiveServer::setPose(const geometry_msgs::PoseStampedConstPtr &msg_ptr, bool for_interactive_control){
205  setPoseWithTfTransformation(tobject, *msg_ptr, for_interactive_control);
206  std_msgs::Header header = msg_ptr->header;
207  header.frame_id = tobject->getFrameId();
208  server_->setPose(focus_object_marker_name_, tobject->pose_, header);
211 }
212 
213 bool TransformableInteractiveServer::getPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req,jsk_interactive_marker::GetTransformableMarkerPose::Response &res, bool for_interactive_control)
214 {
215  TransformableObject* tobject;
216  geometry_msgs::PoseStamped transformed_pose_stamped;
217  if(req.target_name.compare(std::string("")) == 0){
220  }else{
221  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
222  tobject = transformable_objects_map_[req.target_name];
223  }
224  transformed_pose_stamped.header.stamp = ros::Time::now();
225  transformed_pose_stamped.header.frame_id = tobject->frame_id_;
226  transformed_pose_stamped.pose = tobject->getPose(for_interactive_control);
227  res.pose_stamped = transformed_pose_stamped;
228  return true;
229 }
230 
231 bool TransformableInteractiveServer::setPoseService(jsk_interactive_marker::SetTransformableMarkerPose::Request &req,jsk_interactive_marker::SetTransformableMarkerPose::Response &res, bool for_interactive_control)
232 {
233  TransformableObject* tobject;
234  geometry_msgs::PoseStamped transformed_pose_stamped;
235  if(req.target_name.compare(std::string("")) == 0){
238  }else{
239  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
240  focus_object_marker_name_ = req.target_name;
241  tobject = transformable_objects_map_[req.target_name];
242  }
243  if(setPoseWithTfTransformation(tobject, req.pose_stamped, for_interactive_control)){
244  std_msgs::Header header = req.pose_stamped.header;
245  header.frame_id = tobject->getFrameId();
246  server_->setPose(focus_object_marker_name_, tobject->pose_, header);
249  }
250  return true;
251 }
252 
253 bool TransformableInteractiveServer::getColorService(jsk_interactive_marker::GetTransformableMarkerColor::Request &req,jsk_interactive_marker::GetTransformableMarkerColor::Response &res)
254 {
255  TransformableObject* tobject;
256  if(req.target_name.compare(std::string("")) == 0){
259  }else{
260  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
261  tobject = transformable_objects_map_[req.target_name];
262  }
263  tobject->getRGBA(res.color.r, res.color.g, res.color.b, res.color.a);
264  return true;
265 }
266 
267 bool TransformableInteractiveServer::setColorService(jsk_interactive_marker::SetTransformableMarkerColor::Request &req,jsk_interactive_marker::SetTransformableMarkerColor::Response &res)
268 {
269  TransformableObject* tobject;
270  if(req.target_name.compare(std::string("")) == 0){
273  }else{
274  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
275  tobject = transformable_objects_map_[req.target_name];
276  }
277  tobject->setRGBA(req.color.r, req.color.g, req.color.b, req.color.a);
278  updateTransformableObject(tobject);
279  return true;
280 }
281 
282 bool TransformableInteractiveServer::getFocusService(jsk_interactive_marker::GetTransformableMarkerFocus::Request &req,jsk_interactive_marker::GetTransformableMarkerFocus::Response &res)
283 {
284  res.target_name = focus_object_marker_name_;
285  return true;
286 }
287 
288 bool TransformableInteractiveServer::setFocusService(jsk_interactive_marker::SetTransformableMarkerFocus::Request &req,jsk_interactive_marker::SetTransformableMarkerFocus::Response &res)
289 {
290  focus_object_marker_name_ = req.target_name;
294  return true;
295 }
296 
297 bool TransformableInteractiveServer::getTypeService(jsk_interactive_marker::GetType::Request &req,jsk_interactive_marker::GetType::Response &res)
298 {
299  TransformableObject* tobject;
300  if(req.target_name.compare(std::string("")) == 0){
303  res.type = tobject->type_;
304  }else{
305  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
306  tobject = transformable_objects_map_[req.target_name];
307  res.type = tobject->type_;
308  }
309  return true;
310 }
311 
312 bool TransformableInteractiveServer::getExistenceService(jsk_interactive_marker::GetTransformableMarkerExistence::Request &req,jsk_interactive_marker::GetTransformableMarkerExistence::Response &res)
313 {
314  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) {
315  res.existence = false;
316  } else {
317  res.existence = true;
318  }
319  return true;
320 }
321 
322 bool TransformableInteractiveServer::setDimensionsService(jsk_interactive_marker::SetMarkerDimensions::Request &req,jsk_interactive_marker::SetMarkerDimensions::Response &res)
323 {
324  TransformableObject* tobject;
325  if(req.target_name.compare(std::string("")) == 0){
328  }else{
329  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
330  tobject = transformable_objects_map_[req.target_name];
331  }
332  if (tobject) {
333  if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
334  tobject->setXYZ(req.dimensions.x, req.dimensions.y, req.dimensions.z);
335  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
336  tobject->setRZ(req.dimensions.radius, req.dimensions.z);
337  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
338  tobject->setRSR(req.dimensions.radius, req.dimensions.small_radius);
339  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
340  }
342  updateTransformableObject(tobject);
343  }
344  return true;
345 }
346 
347 bool TransformableInteractiveServer::getDimensionsService(jsk_interactive_marker::GetMarkerDimensions::Request &req,jsk_interactive_marker::GetMarkerDimensions::Response &res)
348 {
349  TransformableObject* tobject;
350  if(req.target_name.compare(std::string("")) == 0){
353  }else{
354  if (transformable_objects_map_.find(req.target_name) == transformable_objects_map_.end()) { return true; }
355  tobject = transformable_objects_map_[req.target_name];
356  }
357  if (tobject) {
358  if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
359  tobject->getXYZ(res.dimensions.x, res.dimensions.y, res.dimensions.z);
360  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
361  tobject->getRZ(res.dimensions.radius, res.dimensions.z);
362  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
363  tobject->getRSR(res.dimensions.radius, res.dimensions.small_radius);
364  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
365  }
366 
367  res.dimensions.type = tobject->getType();
368  }
369  return true;
370 }
371 
372 bool TransformableInteractiveServer::hideService(std_srvs::Empty::Request& req,
373  std_srvs::Empty::Response& res)
374 {
375  for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin();
376  itpairstri != transformable_objects_map_.end();
377  ++itpairstri) {
378  TransformableObject* tobject = itpairstri->second;
379  tobject->setDisplayInteractiveManipulator(false);
380  updateTransformableObject(tobject);
381  }
382  return true;
383 }
384 
385 bool TransformableInteractiveServer::showService(std_srvs::Empty::Request& req,
386  std_srvs::Empty::Response& res)
387 {
388  for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin();
389  itpairstri != transformable_objects_map_.end();
390  ++itpairstri) {
391  TransformableObject* tobject = itpairstri->second;
392  tobject->setDisplayInteractiveManipulator(true);
393  updateTransformableObject(tobject);
394  }
395  return true;
396 }
398 {
399  TransformableObject* tobject;
402  if (tobject) {
403  jsk_interactive_marker::MarkerDimensions marker_dimensions;
404  if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
405  tobject->getXYZ(marker_dimensions.x, marker_dimensions.y, marker_dimensions.z);
406  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
407  tobject->getRZ(marker_dimensions.radius, marker_dimensions.z);
408  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
409  tobject->getRSR(marker_dimensions.radius, marker_dimensions.small_radius);
410  } else if (tobject->getType() == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
411  }
412  marker_dimensions.type = tobject->type_;
413  marker_dimensions_pub_.publish(marker_dimensions);
414  }
415 }
416 
417 bool TransformableInteractiveServer::requestMarkerOperateService(jsk_rviz_plugins::RequestMarkerOperate::Request &req,jsk_rviz_plugins::RequestMarkerOperate::Response &res)
418 {
419  switch(req.operate.action){
420  case jsk_rviz_plugins::TransformableMarkerOperate::INSERT:
421  // validation
422  if (req.operate.name.empty()) {
423  ROS_ERROR("Non empty name is required to insert object.");
424  return false;
425  }
426 
427  if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
428  insertNewBox(req.operate.frame_id, req.operate.name, req.operate.description);
429  } else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
430  insertNewCylinder(req.operate.frame_id, req.operate.name, req.operate.description);
431  } else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
432  insertNewTorus(req.operate.frame_id, req.operate.name, req.operate.description);
433  } else if (req.operate.type == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
434  insertNewMesh(req.operate.frame_id, req.operate.name, req.operate.description, req.operate.mesh_resource, req.operate.mesh_use_embedded_materials);
435  }
436  return true;
437  break;
438  case jsk_rviz_plugins::TransformableMarkerOperate::ERASE:
439  eraseObject(req.operate.name);
440  return true;
441  break;
442  case jsk_rviz_plugins::TransformableMarkerOperate::ERASEALL:
443  eraseAllObject();
444  return true;
445  break;
446  case jsk_rviz_plugins::TransformableMarkerOperate::ERASEFOCUS:
448  return true;
449  break;
450  case jsk_rviz_plugins::TransformableMarkerOperate::COPY:
453  if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::BOX) {
454  float x, y, z;
455  tobject->getXYZ(x, y, z);
456  insertNewBox(tobject->frame_id_, req.operate.name, req.operate.description);
457  new_tobject = transformable_objects_map_[req.operate.name];
458  new_tobject->setXYZ(x, y, z);
459  new_tobject->setPose(tobject->getPose());
460  } else if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::CYLINDER) {
461  float r, z;
462  tobject->getRZ(r, z);
463  insertNewCylinder(tobject->frame_id_, req.operate.name, req.operate.description);
464  new_tobject = transformable_objects_map_[req.operate.name];
465  new_tobject->setRZ(r, z);
466  new_tobject->setPose(tobject->getPose());
467  } else if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::TORUS) {
468  float r, sr;
469  tobject->getRSR(r, sr);
470  insertNewTorus(tobject->frame_id_, req.operate.name, req.operate.description);
471  new_tobject = transformable_objects_map_[req.operate.name];
472  new_tobject->setRSR(r, sr);
473  new_tobject->setPose(tobject->getPose());
474  } else if (tobject->type_ == jsk_rviz_plugins::TransformableMarkerOperate::MESH_RESOURCE) {
475  insertNewMesh(tobject->frame_id_, req.operate.name, req.operate.description, req.operate.mesh_resource, req.operate.mesh_use_embedded_materials);
476  new_tobject = transformable_objects_map_[req.operate.name];
477  new_tobject->setPose(tobject->getPose());
478  }
479  float r, g, b, a;
480  tobject->getRGBA(r, g, b, a);
481  new_tobject->setRGBA(r, g, b, a);
482  return true;
483  break;
484  };
485  return false;
486 }
487 
488 void TransformableInteractiveServer::addPose(geometry_msgs::Pose msg){
491  tobject->addPose(msg,false);
492  updateTransformableObject(tobject);
493 }
494 
498  tobject->addPose(msg,true);
499  updateTransformableObject(tobject);
500 }
501 
505  geometry_msgs::Pose pose = tobject->getPose(); //reserve marker pose
506  tobject->control_offset_pose_ = msg;
507  updateTransformableObject(tobject);
508  tobject->setPose(pose);
509  std_msgs::Header header;
510  header.frame_id = tobject->getFrameId();
511  header.stamp = ros::Time::now();
512  server_->setPose(focus_object_marker_name_, tobject->pose_, header);
515 }
516 
518  const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback,
519  const bool enable) {
521  tobject->setDisplayInteractiveManipulator(enable);
522  updateTransformableObject(tobject);
523 }
524 
526  for (std::map<string, TransformableObject* >::iterator it = transformable_objects_map_.begin();
527  it != transformable_objects_map_.end(); it++) {
528  std::string object_name = it->first;
529  TransformableObject* tobject = it->second;
530  if (config_.display_interactive_manipulator && config_.display_interactive_manipulator_only_selected) {
531  // display interactive manipulator only for the focused object
533  }
534  if (config_.display_description_only_selected) {
535  // display description only for the focused object
536  tobject->setDisplayDescription(object_name == focus_object_marker_name_);
537  }
538  updateTransformableObject(tobject);
539  }
540 }
541 
543  jsk_rviz_plugins::OverlayText focus_text;
544  focus_text.text = focus_object_marker_name_;
545  focus_text.top = 0;
546  focus_text.left = 0;
547  focus_text.width = 300;
548  focus_text.height = 50;
549  focus_text.bg_color.r = 0.9;
550  focus_text.bg_color.b = 0.9;
551  focus_text.bg_color.g = 0.9;
552  focus_text.bg_color.a = 0.1;
553  focus_text.fg_color.r = 0.3;
554  focus_text.fg_color.g = 0.3;
555  focus_text.fg_color.b = 0.8;
556  focus_text.fg_color.a = 1;
557  focus_text.line_width = 1;
558  focus_text.text_size = 30;
559  focus_name_text_pub_.publish(focus_text);
560 }
561 
563  geometry_msgs::Pose target_pose;
564  std::stringstream ss;
566  target_pose = transformable_objects_map_[focus_object_marker_name_]->getPose();
567  ss << "Pos x: " << target_pose.position.x << " y: " << target_pose.position.y << " z: " << target_pose.position.z
568  << std::endl
569  << "Ori x: " << target_pose.orientation.x << " y: " << target_pose.orientation.y << " z: " << target_pose.orientation.z << " w: " << target_pose.orientation.w;
570  }
571 
572  jsk_rviz_plugins::OverlayText focus_pose;
573  focus_pose.text = ss.str();
574  focus_pose.top = 50;
575  focus_pose.left = 0;
576  focus_pose.width = 500;
577  focus_pose.height = 50;
578  focus_pose.bg_color.r = 0.9;
579  focus_pose.bg_color.b = 0.9;
580  focus_pose.bg_color.g = 0.9;
581  focus_pose.bg_color.a = 0.1;
582  focus_pose.fg_color.r = 0.8;
583  focus_pose.fg_color.g = 0.3;
584  focus_pose.fg_color.b = 0.3;
585  focus_pose.fg_color.a = 1;
586  focus_pose.line_width = 1;
587  focus_pose.text_size = 15;
588  focus_pose_text_pub_.publish(focus_pose);
589 }
590 
592  std_msgs::String msg;
593  msg.data = focus_object_marker_name_;
595 }
596 
597 void TransformableInteractiveServer::insertNewBox(std::string frame_id, std::string name, std::string description)
598 {
599  TransformableBox* transformable_box = new TransformableBox(0.45, 0.45, 0.45, 0.5, 0.5, 0.5, 1.0, frame_id, name, description);
600  insertNewObject(transformable_box, name);
601 }
602 
603 void TransformableInteractiveServer::insertNewCylinder( std::string frame_id, std::string name, std::string description)
604 {
605  TransformableCylinder* transformable_cylinder = new TransformableCylinder(0.45, 0.45, 0.5, 0.5, 0.5, 1.0, frame_id, name, description);
606  insertNewObject(transformable_cylinder, name);
607 }
608 
609 void TransformableInteractiveServer::insertNewTorus( std::string frame_id, std::string name, std::string description)
610 {
611  TransformableTorus* transformable_torus = new TransformableTorus(0.45, 0.2, torus_udiv_, torus_vdiv_, 0.5, 0.5, 0.5, 1.0, frame_id, name, description);
612  insertNewObject(transformable_torus, name);
613 }
614 
615 void TransformableInteractiveServer::insertNewMesh( std::string frame_id, std::string name, std::string description, std::string mesh_resource, bool mesh_use_embedded_materials)
616 {
617  TransformableMesh* transformable_mesh = new TransformableMesh(frame_id, name, description, mesh_resource, mesh_use_embedded_materials);
618  insertNewObject(transformable_mesh, name);
619 }
620 
622 {
624  visualization_msgs::InteractiveMarker int_marker = tobject->getInteractiveMarker();
625  transformable_objects_map_[name] = tobject;
626  server_->insert(int_marker, boost::bind( &TransformableInteractiveServer::processFeedback,this, _1));
627  yaml_menu_handler_ptr_->applyMenu(server_, name);
629 
634 }
635 
637 {
638  InteractiveSettingConfig config(config_);
639  if (config.display_interactive_manipulator && !config.display_interactive_manipulator_only_selected) {
640  config.display_interactive_manipulator = true;
641  } else {
642  config.display_interactive_manipulator = false;
643  }
644  tobject->setInteractiveMarkerSetting(config);
645 }
646 
648 {
649  server_->erase(name);
651  if (focus_object_marker_name_.compare(name) == 0) {
656  }
657  delete transformable_objects_map_[name];
658  transformable_objects_map_.erase(name);
659 }
660 
662 {
663  for (std::map<string, TransformableObject* >::iterator itpairstri = transformable_objects_map_.begin(); itpairstri != transformable_objects_map_.end(); itpairstri++) {
664  eraseObject(itpairstri->first);
665  }
666 }
667 
669 {
671 }
672 
674 {
677  tobject->publishTF();
678 }
679 
680 bool TransformableInteractiveServer::setPoseWithTfTransformation(TransformableObject* tobject, geometry_msgs::PoseStamped pose_stamped, bool for_interactive_control)
681 {
682  try {
683  geometry_msgs::PoseStamped transformed_pose_stamped;
684  jsk_interactive_marker::PoseStampedWithName transformed_pose_stamped_with_name;
686  if (strict_tf_) {
687  stamp = pose_stamped.header.stamp;
688  }
689  else {
690  stamp = ros::Time(0.0);
691  pose_stamped.header.stamp = stamp;
692  }
693  if (!strict_tf_ || tf_listener_->waitForTransform(tobject->getFrameId(),
694  pose_stamped.header.frame_id, stamp, ros::Duration(1.0))) {
695  tf_listener_->transformPose(tobject->getFrameId(), pose_stamped, transformed_pose_stamped);
696  tobject->setPose(transformed_pose_stamped.pose, for_interactive_control);
697  transformed_pose_stamped.pose=tobject->getPose(true);
698  pose_pub_.publish(transformed_pose_stamped);
699  transformed_pose_stamped_with_name.pose = transformed_pose_stamped;
700  transformed_pose_stamped_with_name.name = tobject->name_;
701  //transformed_pose_stamped_with_name.name = focus_object_marker_name_;
702  pose_with_name_pub_.publish(transformed_pose_stamped_with_name);
703  }
704  else {
705  ROS_ERROR("failed to lookup transform %s -> %s", tobject->getFrameId().c_str(),
706  pose_stamped.header.frame_id.c_str());
707  return false;
708  }
709  }
710  catch (tf2::ConnectivityException &e)
711  {
712  ROS_ERROR("Transform error: %s", e.what());
713  return false;
714  }
716  {
717  ROS_ERROR("Transform error: %s", e.what());
718  return false;
719  }
720  return true;
721 }
722 
724  ros::spin();
725 }
msg
f
geometry_msgs::Pose getPose(bool for_interactive_control=false)
virtual void setRGBA(float r, float g, float b, float a)
void publish(const boost::shared_ptr< M > &message) const
x
bool setDimensionsService(jsk_interactive_marker::SetMarkerDimensions::Request &req, jsk_interactive_marker::SetMarkerDimensions::Response &res)
void enableInteractiveManipulatorDisplay(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback, const bool enable)
bool getTypeService(jsk_interactive_marker::GetType::Request &req, jsk_interactive_marker::GetType::Response &res)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void processFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
config
void insertNewCylinder(std::string frame_id, std::string name, std::string description)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
virtual void getRGBA(float &r, float &g, float &b, float &a)
pose
bool showService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
ROSCPP_DECL void spin(Spinner &spinner)
void setPose(geometry_msgs::Pose pose, bool for_interactive_control=false)
virtual bool setX(std_msgs::Float32 recieve_val)
virtual void getXYZ(float &x, float &y, float &z)
bool setPoseWithTfTransformation(TransformableObject *tobject, geometry_msgs::PoseStamped pose_stamped, bool for_interactive_control=false)
void setPose(const geometry_msgs::PoseStampedConstPtr &msg_ptr, bool for_interactive_control=false)
void addPose(geometry_msgs::Pose msg, bool relative=false)
y
bool getExistenceService(jsk_interactive_marker::GetTransformableMarkerExistence::Request &req, jsk_interactive_marker::GetTransformableMarkerExistence::Response &res)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool setColorService(jsk_interactive_marker::SetTransformableMarkerColor::Request &req, jsk_interactive_marker::SetTransformableMarkerColor::Response &res)
bool getPoseService(jsk_interactive_marker::GetTransformableMarkerPose::Request &req, jsk_interactive_marker::GetTransformableMarkerPose::Response &res, bool for_interactive_control)
header
void insertNewMesh(std::string frame_id, std::string name, std::string description, std::string mesh_resource, bool mesh_use_embedded_materials)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool requestMarkerOperateService(jsk_rviz_plugins::RequestMarkerOperate::Request &req, jsk_rviz_plugins::RequestMarkerOperate::Response &res)
bool hideService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
bool getDimensionsService(jsk_interactive_marker::GetMarkerDimensions::Request &req, jsk_interactive_marker::GetMarkerDimensions::Response &res)
void insertNewBox(std::string frame_id, std::string name, std::string description)
mutex_t * lock
virtual void getRSR(float &r, float &sr)
bool getFocusService(jsk_interactive_marker::GetTransformableMarkerFocus::Request &req, jsk_interactive_marker::GetTransformableMarkerFocus::Response &res)
INTERACTIVE_MARKERS_PUBLIC void insert(const visualization_msgs::InteractiveMarker &int_marker)
INTERACTIVE_MARKERS_PUBLIC void applyChanges()
void insertNewObject(TransformableObject *tobject, std::string name)
static Time now()
bool setFocusService(jsk_interactive_marker::SetTransformableMarkerFocus::Request &req, jsk_interactive_marker::SetTransformableMarkerFocus::Response &res)
jsk_interactive_marker::InteractiveSettingConfig config_
INTERACTIVE_MARKERS_PUBLIC bool setPose(const std::string &name, const geometry_msgs::Pose &pose, const std_msgs::Header &header=std_msgs::Header())
void insertNewTorus(std::string frame_id, std::string name, std::string description)
virtual void getRZ(float &r, float &z)
virtual bool setZ(std_msgs::Float32 recieve_val)
void setInteractiveMarkerSetting(const InteractiveSettingConfig &config)
std::shared_ptr< dynamic_reconfigure::Server< InteractiveSettingConfig > > config_srv_
bool setPoseService(jsk_interactive_marker::SetTransformableMarkerPose::Request &req, jsk_interactive_marker::SetTransformableMarkerPose::Response &res, bool for_interactive_control)
#define ROS_ERROR(...)
INTERACTIVE_MARKERS_PUBLIC bool erase(const std::string &name)
virtual void configCallback(InteractiveSettingConfig &config, uint32_t level)
virtual void setXYZ(float x, float y, float z)
bool getColorService(jsk_interactive_marker::GetTransformableMarkerColor::Request &req, jsk_interactive_marker::GetTransformableMarkerColor::Response &res)
z
visualization_msgs::InteractiveMarker getInteractiveMarker()
char a[26]
virtual bool setSmallRadius(std_msgs::Float32 recieve_val)
virtual bool setRadius(std_msgs::Float32 recieve_val)
virtual bool setY(std_msgs::Float32 recieve_val)


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