interactive_marker_utils.cpp
Go to the documentation of this file.
1 #include <jsk_interactive_marker/interactive_marker_utils.h>
2 #include <boost/filesystem/operations.hpp>
3 #include <iostream>
4 #include <stdlib.h>
5 #include <ros/package.h>
6 #include "urdf_parser/urdf_parser.h"
7 
8 #include <kdl/frames_io.hpp>
9 #include <tf_conversions/tf_kdl.h>
10 
11 using namespace urdf;
12 using namespace std;
13 using namespace boost;
14 using namespace boost::filesystem;
15 //using namespace im_utils;
16 
17 namespace im_utils {
18 
19  //transform msgs
20  geometry_msgs::Transform Pose2Transform( const geometry_msgs::Pose pose_msg){
21  geometry_msgs::Transform tf_msg;
22  tf_msg.translation.x = pose_msg.position.x;
23  tf_msg.translation.y = pose_msg.position.y;
24  tf_msg.translation.z = pose_msg.position.z;
25  tf_msg.rotation = pose_msg.orientation;
26  return tf_msg;
27  }
28 
29  geometry_msgs::Pose Transform2Pose( const geometry_msgs::Transform tf_msg){
30  geometry_msgs::Pose pose_msg;
31  pose_msg.position.x = tf_msg.translation.x;
32  pose_msg.position.y = tf_msg.translation.y;
33  pose_msg.position.z = tf_msg.translation.z;
34  pose_msg.orientation = tf_msg.rotation;
35  return pose_msg;
36  }
37 
38  geometry_msgs::Pose UrdfPose2Pose( const urdf::Pose pose){
39  geometry_msgs::Pose p_msg;
40  double x, y, z, w;
41  pose.rotation.getQuaternion(x,y,z,w);
42  p_msg.orientation.x = x;
43  p_msg.orientation.y = y;
44  p_msg.orientation.z = z;
45  p_msg.orientation.w = w;
46 
47  p_msg.position.x = pose.position.x;
48  p_msg.position.y = pose.position.y;
49  p_msg.position.z = pose.position.z;
50 
51  return p_msg;
52  }
53 
54 
55  visualization_msgs::InteractiveMarkerControl makeCylinderMarkerControl(const geometry_msgs::PoseStamped &stamped, double length, double radius, const std_msgs::ColorRGBA &color, bool use_color){
56  visualization_msgs::Marker cylinderMarker;
57 
58  if (use_color) cylinderMarker.color = color;
59  cylinderMarker.type = visualization_msgs::Marker::CYLINDER;
60  cylinderMarker.scale.x = radius * 2;
61  cylinderMarker.scale.y = radius * 2;
62  cylinderMarker.scale.z = length;
63  cylinderMarker.pose = stamped.pose;
64 
65  visualization_msgs::InteractiveMarkerControl control;
66  control.markers.push_back( cylinderMarker );
67  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
68  control.always_visible = true;
69 
70  return control;
71  }
72 
73  visualization_msgs::InteractiveMarkerControl makeBoxMarkerControl(const geometry_msgs::PoseStamped &stamped, Vector3 dim, const std_msgs::ColorRGBA &color, bool use_color){
74  visualization_msgs::Marker boxMarker;
75 
76  fprintf(stderr, "urdfModelMarker = %f %f %f\n", dim.x, dim.y, dim.z);
77  if (use_color) boxMarker.color = color;
78  boxMarker.type = visualization_msgs::Marker::CUBE;
79  boxMarker.scale.x = dim.x;
80  boxMarker.scale.y = dim.y;
81  boxMarker.scale.z = dim.z;
82  boxMarker.pose = stamped.pose;
83 
84  visualization_msgs::InteractiveMarkerControl control;
85  control.markers.push_back( boxMarker );
86  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
87  control.always_visible = true;
88 
89  return control;
90  }
91 
92  visualization_msgs::InteractiveMarkerControl makeSphereMarkerControl(const geometry_msgs::PoseStamped &stamped, double rad, const std_msgs::ColorRGBA &color, bool use_color){
93  visualization_msgs::Marker sphereMarker;
94 
95  if (use_color) sphereMarker.color = color;
96  sphereMarker.type = visualization_msgs::Marker::SPHERE;
97  sphereMarker.scale.x = rad * 2;
98  sphereMarker.scale.y = rad * 2;
99  sphereMarker.scale.z = rad * 2;
100  sphereMarker.pose = stamped.pose;
101 
102  visualization_msgs::InteractiveMarkerControl control;
103  control.markers.push_back( sphereMarker );
104  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
105  control.always_visible = true;
106 
107  return control;
108  }
109 
110 
111 
112  visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color, bool use_color){
113  visualization_msgs::Marker meshMarker;
114 
115  if (use_color) meshMarker.color = color;
116  meshMarker.mesh_resource = mesh_resource;
117  meshMarker.mesh_use_embedded_materials = !use_color;
118  meshMarker.type = visualization_msgs::Marker::MESH_RESOURCE;
119 
120  meshMarker.scale = scale;
121  meshMarker.pose = stamped.pose;
122  visualization_msgs::InteractiveMarkerControl control;
123  control.markers.push_back( meshMarker );
124  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
125  control.always_visible = true;
126 
127  return control;
128  }
129 
130  visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale)
131  {
132  std_msgs::ColorRGBA color;
133  color.r = 0;
134  color.g = 0;
135  color.b = 0;
136  color.a = 0;
137  return makeMeshMarkerControl(mesh_resource, stamped, scale, color, false);
138  }
139 
140  visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource,
141  const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color)
142  {
143  return makeMeshMarkerControl(mesh_resource, stamped, scale, color, true);
144  }
145 
146  void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, LinkConstSharedPtr link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale){
147  addMeshLinksControl(im, link, previous_frame, use_color, color, scale, true);
148  }
149 
150  void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, LinkConstSharedPtr link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale, bool root){
151  if(!root && link->parent_joint){
152  KDL::Frame parent_to_joint_frame;
153  geometry_msgs::Pose parent_to_joint_pose = UrdfPose2Pose(link->parent_joint->parent_to_joint_origin_transform);
154  tf::poseMsgToKDL(parent_to_joint_pose, parent_to_joint_frame);
155  previous_frame = previous_frame * parent_to_joint_frame;
156  }
157 
158  // KDL::Frame pose_frame, offset_frame;
159  // tf::poseMsgToKDL(pose, pose_frame);
160  // tf::poseMsgToKDL(root_offset_, offset_frame);
161  // pose_frame = pose_frame * offset_frame;
162 
163  geometry_msgs::PoseStamped ps;
164  //link_array
165  std::vector<VisualSharedPtr> visual_array;
166  if(link->visual_array.size() != 0){
167  visual_array = link->visual_array;
168  }else if(link->visual.get() != NULL){
169  visual_array.push_back(link->visual);
170  }
171  for(int i=0; i<visual_array.size(); i++){
172  VisualSharedPtr link_visual = visual_array[i];
173  if(link_visual.get() != NULL && link_visual->geometry.get() != NULL){
174  visualization_msgs::InteractiveMarkerControl meshControl;
175  if(link_visual->geometry->type == Geometry::MESH){
176 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
177  MeshConstSharedPtr mesh = std::static_pointer_cast<const Mesh>(link_visual->geometry);
178 #else
179  MeshConstSharedPtr mesh = boost::static_pointer_cast<const Mesh>(link_visual->geometry);
180 #endif
181  string model_mesh_ = mesh->filename;
182  model_mesh_ = getRosPathFromModelPath(model_mesh_);
183 
184  //ps.pose = UrdfPose2Pose(link_visual->origin);
185  KDL::Frame pose_frame, origin_frame;
186 
187  tf::poseMsgToKDL(UrdfPose2Pose(link_visual->origin), origin_frame);
188  pose_frame = previous_frame * origin_frame;
189  geometry_msgs::Pose pose;
190  tf::poseKDLToMsg(pose_frame, pose);
191  ps.pose = pose;
192 
193  cout << "mesh_file:" << model_mesh_ << endl;
194 
195  geometry_msgs::Vector3 mesh_scale;
196  mesh_scale.x = mesh->scale.x * scale;
197  mesh_scale.y = mesh->scale.y * scale;
198  mesh_scale.z = mesh->scale.z * scale;
199  if(use_color){
200  meshControl = makeMeshMarkerControl(model_mesh_, ps, mesh_scale, color);
201  }else{
202  meshControl = makeMeshMarkerControl(model_mesh_, ps, mesh_scale);
203  }
204  }else if(link_visual->geometry->type == Geometry::CYLINDER){
205 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
206  CylinderConstSharedPtr cylinder = std::static_pointer_cast<const Cylinder>(link_visual->geometry);
207 #else
208  CylinderConstSharedPtr cylinder = boost::static_pointer_cast<const Cylinder>(link_visual->geometry);
209 #endif
210  std::cout << "cylinder " << link->name;
211  ps.pose = UrdfPose2Pose(link_visual->origin);
212  double length = cylinder->length;
213  double radius = cylinder->radius;
214  std::cout << ", length = " << length << ", radius " << radius << std::endl;
215  if(use_color){
216  meshControl = makeCylinderMarkerControl(ps, length, radius, color, true);
217  }else{
218  meshControl = makeCylinderMarkerControl(ps, length, radius, color, true);
219  }
220  }else if(link_visual->geometry->type == Geometry::BOX){
221 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
222  BoxConstSharedPtr box = std::static_pointer_cast<const Box>(link_visual->geometry);
223 #else
224  BoxConstSharedPtr box = boost::static_pointer_cast<const Box>(link_visual->geometry);
225 #endif
226  std::cout << "box " << link->name;
227  ps.pose = UrdfPose2Pose(link_visual->origin);
228  Vector3 dim = box->dim;
229  std::cout << ", dim = " << dim.x << ", " << dim.y << ", " << dim.z << std::endl;
230  if(use_color){
231  meshControl = makeBoxMarkerControl(ps, dim, color, true);
232  }else{
233  meshControl = makeBoxMarkerControl(ps, dim, color, true);
234  }
235  }else if(link_visual->geometry->type == Geometry::SPHERE){
236 #if ROS_VERSION_MINIMUM(1,14,0) // melodic
237  SphereConstSharedPtr sphere = std::static_pointer_cast<const Sphere>(link_visual->geometry);
238 #else
239  SphereConstSharedPtr sphere = boost::static_pointer_cast<const Sphere>(link_visual->geometry);
240 #endif
241  ps.pose = UrdfPose2Pose(link_visual->origin);
242  double rad = sphere->radius;
243  if(use_color){
244  meshControl = makeSphereMarkerControl(ps, rad, color, true);
245  }else{
246  meshControl = makeSphereMarkerControl(ps, rad, color, true);
247  }
248  }
249  im.controls.push_back( meshControl );
250 
251  }
252  }
253  for (std::vector<LinkSharedPtr>::const_iterator child = link->child_links.begin(); child != link->child_links.end(); child++){
254  addMeshLinksControl(im, *child, previous_frame, use_color, color, scale, false);
255  }
256  }
257 
258  ModelInterfaceSharedPtr getModelInterface(std::string model_file){
259  ModelInterfaceSharedPtr model;
260  model_file = getFilePathFromRosPath(model_file);
261  model_file = getFullPathFromModelPath(model_file);
262  std::string xml_string;
263  std::fstream xml_file(model_file.c_str(), std::fstream::in);
264  while ( xml_file.good() )
265  {
266  std::string line;
267  std::getline( xml_file, line);
268  xml_string += (line + "\n");
269  }
270  xml_file.close();
271 
272  std::cout << "model_file:" << model_file << std::endl;
273  model = parseURDF(xml_string);
274  if (!model){
275  std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
276  }
277  return model;
278  }
279 
280  //sample program
281  visualization_msgs::InteractiveMarker makeLinksMarker(LinkConstSharedPtr link, bool use_color, std_msgs::ColorRGBA color, geometry_msgs::PoseStamped marker_ps, geometry_msgs::Pose origin_pose)
282  {
283  visualization_msgs::InteractiveMarker int_marker;
284  int_marker.header = marker_ps.header;
285  int_marker.pose = marker_ps.pose;
286 
287  int_marker.name = "sample";
288  int_marker.scale = 1.0;
289 
290  KDL::Frame origin_frame;
291  tf::poseMsgToKDL(origin_pose, origin_frame);
292  addMeshLinksControl(int_marker, link, origin_frame, use_color, color, 1.0);
293  return int_marker;
294 
295  }
296 
297 
298 
299  visualization_msgs::InteractiveMarker makeFingerControlMarker(const char *name, geometry_msgs::PoseStamped ps){
300  visualization_msgs::InteractiveMarker int_marker;
301  int_marker.name = name;
302  int_marker.header = ps.header;
303  int_marker.pose = ps.pose;
304  int_marker.scale = 0.5;
305 
306  visualization_msgs::InteractiveMarkerControl control;
307 
308  //control.orientation_mode = visualization_msgs::InteractiveMarkerControl::VIEW_FACING;
309  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
310  control.orientation.w = 1;
311  control.orientation.x = 0;
312  control.orientation.y = 0;
313  control.orientation.z = 1;
314 
315  int_marker.controls.push_back(control);
316 
317  return int_marker;
318 
319  }
320 
321  /*
322 
323  visualization_msgs::InteractiveMarker makeSandiaHandMarker(geometry_msgs::PoseStamped ps){
324  visualization_msgs::InteractiveMarker int_marker;
325  int_marker.header = ps.header;
326  int_marker.pose = ps.pose;
327 
328  visualization_msgs::InteractiveMarkerControl control;
329 
330  control.markers.push_back(makeSandiaFinger0Marker("/right_f0_0"));
331  int_marker.controls.push_back(control);
332 
333  control.markers.clear();
334  control.markers.push_back(makeSandiaFinger1Marker("/right_f0_1"));
335  int_marker.controls.push_back(control);
336 
337  control.markers.clear();
338  control.markers.push_back(makeSandiaFinger2Marker("/right_f0_2"));
339  int_marker.controls.push_back(control);
340 
341  control.markers.clear();
342  control.markers.push_back(makeSandiaFinger0Marker("/right_f1_0"));
343  int_marker.controls.push_back(control);
344 
345  control.markers.clear();
346  control.markers.push_back(makeSandiaFinger1Marker("/right_f1_1"));
347  int_marker.controls.push_back(control);
348 
349  control.markers.clear();
350  control.markers.push_back(makeSandiaFinger2Marker("/right_f1_2"));
351  int_marker.controls.push_back(control);
352 
353  control.markers.clear();
354  control.markers.push_back(makeSandiaFinger0Marker("/right_f2_0"));
355  int_marker.controls.push_back(control);
356 
357  control.markers.clear();
358  control.markers.push_back(makeSandiaFinger1Marker("/right_f2_1"));
359  int_marker.controls.push_back(control);
360 
361  control.markers.clear();
362  control.markers.push_back(makeSandiaFinger2Marker("/right_f2_2"));
363  int_marker.controls.push_back(control);
364 
365  control.markers.push_back(makeSandiaFinger0Marker("/right_f3_0"));
366  int_marker.controls.push_back(control);
367 
368  control.markers.clear();
369  control.markers.push_back(makeSandiaFinger1Marker("/right_f3_1"));
370  int_marker.controls.push_back(control);
371 
372  control.markers.clear();
373  control.markers.push_back(makeSandiaFinger2Marker("/right_f3_2"));
374  int_marker.controls.push_back(control);
375 
376  return int_marker;
377 
378  }
379 
380  */
381 
382 
383  visualization_msgs::InteractiveMarker makeSandiaHandInteractiveMarker(geometry_msgs::PoseStamped ps, std::string hand, int finger, int link){
384  visualization_msgs::InteractiveMarker int_marker;
385  int_marker.header = ps.header;
386  int_marker.pose = ps.pose;
387 
388 
389  visualization_msgs::InteractiveMarkerControl control;
390  std::stringstream ss;
391  //std::string frame = "/" + hand + "_" + finger + "_" + link;
392  ss << hand << "_f" << finger << "_" << link;
393 
394  int_marker.name = ss.str() + "Marker";
395  int_marker.header.frame_id = ss.str();
396  // std::string frame_id = "odom";
397  std::string frame_id = "utorso";
398  int_marker.header.frame_id = frame_id;
399  std::cout << ss.str() << std::endl;
400 
401  //frame_id = ss.str();
402  switch(link){
403  case 0:
404  control.markers.push_back(makeSandiaFinger0Marker(frame_id));
405  break;
406  case 1:
407  control.markers.push_back(makeSandiaFinger1Marker(frame_id));
408  break;
409  case 2:
410  control.markers.push_back(makeSandiaFinger2Marker(frame_id));
411  break;
412  default:
413  break;
414  }
415  control.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
416  int_marker.controls.push_back(control);
417 
418  return int_marker;
419  }
420 
421 
422 
423  visualization_msgs::Marker makeSandiaFinger0Marker(std::string frame_id){
424  visualization_msgs::Marker marker;
425  marker.header.frame_id = frame_id;
426  //marker.header.frame_id = "odom";
427  marker.type = visualization_msgs::Marker::CYLINDER;
428  marker.action = visualization_msgs::Marker::ADD;
429  marker.pose.position.x = 0;
430  marker.pose.position.y = 0;
431  marker.pose.position.z = 0.003;
432  marker.pose.orientation.x = 0.0;
433  marker.pose.orientation.y = 0.0;
434  marker.pose.orientation.z = 0.0;
435  marker.pose.orientation.w = 1.0;
436  marker.scale.x = 0.03;
437  marker.scale.y = 0.03;
438  marker.scale.z = 0.023;
439  marker.color.a = 1.0;
440  marker.color.r = 0.0;
441  marker.color.g = 1.0;
442  marker.color.b = 0.0;
443 
444  return marker;
445  }
446 
447  visualization_msgs::Marker makeSandiaFinger1Marker(std::string frame_id){
448  visualization_msgs::Marker marker;
449  marker.header.frame_id = frame_id;
450  marker.type = visualization_msgs::Marker::CYLINDER;
451  marker.action = visualization_msgs::Marker::ADD;
452  marker.pose.position.x = 0.024;
453  marker.pose.position.y = 0;
454  marker.pose.position.z = 0;
455  marker.pose.orientation.x = 0.0;
456  marker.pose.orientation.y = 1.0;
457  marker.pose.orientation.z = 0.0;
458  marker.pose.orientation.w = 1.0;
459  marker.scale.x = 0.02;
460  marker.scale.y = 0.02;
461  marker.scale.z = 0.067;
462  marker.color.a = 1.0;
463  marker.color.r = 0.0;
464  marker.color.g = 1.0;
465  marker.color.b = 0.0;
466 
467  return marker;
468  }
469 
470  visualization_msgs::Marker makeSandiaFinger2Marker(std::string frame_id){
471  visualization_msgs::Marker marker;
472  marker.header.frame_id = frame_id;
473  marker.type = visualization_msgs::Marker::CYLINDER;
474  marker.action = visualization_msgs::Marker::ADD;
475  marker.pose.position.x = 0.024;
476  marker.pose.position.y = 0;
477  marker.pose.position.z = 0;
478  marker.pose.orientation.x = 0.0;
479  marker.pose.orientation.y = 1.0;
480  marker.pose.orientation.z = 0.0;
481  marker.pose.orientation.w = 1.0;
482  marker.scale.x = 0.018;
483  marker.scale.y = 0.018;
484  marker.scale.z = 0.057;
485  marker.color.a = 1.0;
486  marker.color.r = 0.0;
487  marker.color.g = 1.0;
488  marker.color.b = 0.0;
489 
490  return marker;
491  }
492 
493  std::string getRosPathFromModelPath(std::string path){
494  return getRosPathFromFullPath(getFullPathFromModelPath(path));
495  }
496 
497  std::string getRosPathFromFullPath(std::string path){
498  std::string ros_package_path = "";
499  FILE* fp;
500  char buf[1000000];
501 
502  //set $ROS_PACKAGE_PATH
503  if ((fp = popen("echo $ROS_PACKAGE_PATH", "r")) == NULL) {
504  std::cout << "popen error" << std::endl;
505  }
506  while (fgets(buf, sizeof(buf), fp) != NULL) {
507  ros_package_path += buf;
508  }
509  pclose(fp);
510 
511  if( path.find("file://", 0) == 0 ){
512  path.erase(0,7);
513 
514  //trim ros_package_path
515  size_t current = 0, found;
516  while((found = ros_package_path.find_first_of(":", current)) != std::string::npos){
517  std::string search_path = std::string(ros_package_path, current, found - current);
518  current = found + 1;
519  if(path.find(search_path, 0) == 0){
520  path.erase(0, strlen(search_path.c_str()));
521  break;
522  }
523  }
524 
525  std::string tmp[] = {"", "jsk-ros-pkg", "jsk_model_tools"};
526  std::set<std::string> package_blackset(tmp, tmp + sizeof(tmp) / sizeof(tmp[0]));
527 
528  current = 0;
529  while((found = path.find_first_of("/", current)) != std::string::npos){
530  std::string search_path = std::string(path, current, found - current);
531  current = found + 1;
532  std::string package_path;
533 
534  // check brackset
535  if(package_blackset.find(search_path) != package_blackset.end()){
536  continue;
537  }
538 
539  if( search_path != "" && ros::package::getPath(search_path) != ""){
540  return "package://" + search_path + path.erase(0, current-1);
541  }
542  }
543  path = "file://" + path;
544  }
545 
546  return path;
547  }
548 
549  std::string getFullPathFromModelPath(std::string path){
550  std::string gazebo_model_path="";
551 
552  FILE* fp;
553  char buf[1000000];
554 
555  //set $GAZEBO_MODEL_PATH
556  if ((fp = popen("echo $GAZEBO_MODEL_PATH", "r")) == NULL) {
557  std::cout << "popen error" << std::endl;
558  }
559  while (fgets(buf, sizeof(buf), fp) != NULL) {
560  gazebo_model_path += buf;
561  }
562  pclose(fp);
563  if( path.find("model://", 0) == 0 ){
564  path.erase(0,8);
565  // ??
566  //path.erase(0,9);
567 
568  size_t current = 0, found;
569  while((found = gazebo_model_path.find_first_of(":", current)) != std::string::npos){
570  try{
571  std::string search_path = std::string(gazebo_model_path, current, found - current);
572  current = found + 1;
573  recursive_directory_iterator iter = recursive_directory_iterator(search_path);
574  recursive_directory_iterator end = recursive_directory_iterator();
575  for (; iter != end; ++iter) {
576  if (is_regular_file(*iter)) {
577  int locate = iter->path().string().find( path, 0 );
578  if( locate != std::string::npos){
579  //for example file:///hoge/fuga.dae
580  return "file://" + iter->path().string();
581  }
582  }
583  }
584  }catch(...){
585  }
586  }
587  }
588  return path;
589  }
590 
591  //convert package:// path to full path
592  std::string getFilePathFromRosPath( std::string rospath){
593  std::string path = rospath;
594  if (path.find("package://") == 0){
595  path.erase(0, strlen("package://"));
596  size_t pos = path.find("/");
597  if (pos == std::string::npos){
598  std::cout << "Could not parse package:// format" <<std::endl;
599  return "";
600  }
601  std::string package = path.substr(0, pos);
602  path.erase(0, pos);
603  std::string package_path = ros::package::getPath(package);
604  if (package_path.empty())
605  {
606  std::cout << "Package [" + package + "] does not exist" << std::endl;
607  }
608 
609  path = package_path + path;
610  }
611  return path;
612  }
613 
614  geometry_msgs::Pose getPose( XmlRpc::XmlRpcValue val){
615  geometry_msgs::Pose p;
616  if(val.hasMember("position")){
617  XmlRpc::XmlRpcValue pos = val["position"];
618  p.position.x = getXmlValue(pos["x"]);
619  p.position.y = getXmlValue(pos["y"]);
620  p.position.z = getXmlValue(pos["z"]);
621  }else{
622  p.position.x = p.position.y = p.position.z = 0.0;
623  }
624 
625  if(val.hasMember("orientation")){
626  XmlRpc::XmlRpcValue ori = val["orientation"];
627  p.orientation.x = getXmlValue(ori["x"]);
628  p.orientation.y = getXmlValue(ori["y"]);
629  p.orientation.z = getXmlValue(ori["z"]);
630  p.orientation.w = getXmlValue(ori["w"]);
631  }else{
632  p.orientation.x = p.orientation.y = p.orientation.z = 0.0;
633  p.orientation.w = 1.0;
634  }
635 
636  return p;
637  }
638 
639  double getXmlValue( XmlRpc::XmlRpcValue val ){
640  switch(val.getType()){
641  case XmlRpc::XmlRpcValue::TypeInt:
642  return (double)((int)val);
643  case XmlRpc::XmlRpcValue::TypeDouble:
644  return (double)val;
645  default:
646  return 0;
647  }
648  }
649 
650 }
651 
652 
im_utils::makeCylinderMarkerControl
visualization_msgs::InteractiveMarkerControl makeCylinderMarkerControl(const geometry_msgs::PoseStamped &stamped, double length, double radius, const std_msgs::ColorRGBA &color, bool use_color)
Definition: interactive_marker_utils.cpp:55
im_utils::Pose2Transform
geometry_msgs::Transform Pose2Transform(const geometry_msgs::Pose pose_msg)
Definition: interactive_marker_utils.cpp:20
im_utils::getRosPathFromModelPath
std::string getRosPathFromModelPath(std::string path)
Definition: interactive_marker_utils.cpp:493
ros::package::getPath
ROSLIB_DECL std::string getPath(const std::string &package_name)
mesh_resource
mesh_resource
xml_string
def xml_string(rootXml, addHeader=True)
marker
marker
scale
scale
boost::shared_ptr
im_utils::addMeshLinksControl
void addMeshLinksControl(visualization_msgs::InteractiveMarker &im, LinkConstSharedPtr link, KDL::Frame previous_frame, bool use_color, std_msgs::ColorRGBA color, double scale)
Definition: interactive_marker_utils.cpp:146
pos
pos
p
p
w
w
z
z
im_utils::makeLinksMarker
visualization_msgs::InteractiveMarker makeLinksMarker(LinkConstSharedPtr link, bool use_color, std_msgs::ColorRGBA color, geometry_msgs::PoseStamped marker_ps, geometry_msgs::Pose origin_pose)
Definition: interactive_marker_utils.cpp:281
XmlRpc::XmlRpcValue::TypeInt
TypeInt
tf::poseMsgToKDL
void poseMsgToKDL(const geometry_msgs::Pose &m, KDL::Frame &k)
im_utils::makeBoxMarkerControl
visualization_msgs::InteractiveMarkerControl makeBoxMarkerControl(const geometry_msgs::PoseStamped &stamped, Vector3 dim, const std_msgs::ColorRGBA &color, bool use_color)
Definition: interactive_marker_utils.cpp:73
boost
im_utils::makeSphereMarkerControl
visualization_msgs::InteractiveMarkerControl makeSphereMarkerControl(const geometry_msgs::PoseStamped &stamped, double rad, const std_msgs::ColorRGBA &color, bool use_color)
Definition: interactive_marker_utils.cpp:92
getXmlValue
double getXmlValue(XmlRpc::XmlRpcValue val)
Definition: interactive_marker_utils.cpp:639
pose
pose
XmlRpc::XmlRpcValue::TypeDouble
TypeDouble
im_utils::makeMeshMarkerControl
visualization_msgs::InteractiveMarkerControl makeMeshMarkerControl(const std::string &mesh_resource, const geometry_msgs::PoseStamped &stamped, geometry_msgs::Vector3 scale, const std_msgs::ColorRGBA &color, bool use_color)
Definition: interactive_marker_utils.cpp:112
im_utils::makeSandiaFinger1Marker
visualization_msgs::Marker makeSandiaFinger1Marker(std::string frame_id)
Definition: interactive_marker_utils.cpp:447
im_utils::makeFingerControlMarker
visualization_msgs::InteractiveMarker makeFingerControlMarker(const char *name, geometry_msgs::PoseStamped ps)
Definition: interactive_marker_utils.cpp:299
im_utils::makeSandiaHandInteractiveMarker
visualization_msgs::InteractiveMarker makeSandiaHandInteractiveMarker(geometry_msgs::PoseStamped ps, std::string hand, int finger, int link)
Definition: interactive_marker_utils.cpp:383
NULL
#define NULL
interactive_marker_utils.h
x
x
im_utils::getRosPathFromFullPath
std::string getRosPathFromFullPath(std::string path)
Definition: interactive_marker_utils.cpp:497
im_utils::getModelInterface
ModelInterfaceSharedPtr getModelInterface(std::string model_file)
Definition: interactive_marker_utils.cpp:258
end
end
y
y
package.h
XmlRpc::XmlRpcValue::getType
const Type & getType() const
line
im_utils::getFilePathFromRosPath
std::string getFilePathFromRosPath(std::string rospath)
Definition: interactive_marker_utils.cpp:592
XmlRpc::XmlRpcValue::hasMember
bool hasMember(const std::string &name) const
im_utils::UrdfPose2Pose
geometry_msgs::Pose UrdfPose2Pose(const urdf::Pose pose)
Definition: interactive_marker_utils.cpp:38
dummy_camera.frame_id
frame_id
Definition: dummy_camera.py:10
urdf
std
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
im_utils
Definition: interactive_marker_utils.h:47
tf_kdl.h
im_utils::makeSandiaFinger0Marker
visualization_msgs::Marker makeSandiaFinger0Marker(std::string frame_id)
Definition: interactive_marker_utils.cpp:423
radius
GLdouble radius
root
root
package
im_utils::Transform2Pose
geometry_msgs::Pose Transform2Pose(const geometry_msgs::Transform tf_msg)
Definition: interactive_marker_utils.cpp:29
tf::poseKDLToMsg
void poseKDLToMsg(const KDL::Frame &k, geometry_msgs::Pose &m)
im_utils::getFullPathFromModelPath
std::string getFullPathFromModelPath(std::string path)
Definition: interactive_marker_utils.cpp:549
XmlRpc::XmlRpcValue
getPose
geometry_msgs::Pose getPose(XmlRpc::XmlRpcValue val)
Definition: interactive_marker_utils.cpp:614
im_utils::makeSandiaFinger2Marker
visualization_msgs::Marker makeSandiaFinger2Marker(std::string frame_id)
Definition: interactive_marker_utils.cpp:470

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