CoordinateFrameVisualizer.cpp
Go to the documentation of this file.
1 
19 
20 namespace Visualization {
21 
24  {
25  }
26 
28  {
29  }
30 
32  unsigned int& pMarkerId,
33  const boost::shared_ptr<ISM::Pose> pPose,
34  double pSize)
35  {
36  // Check, if publisher is available.
37  if(!pPublisher)
38  throw std::invalid_argument("Empty ros::publisher cannot be used to publish marker messages.");
39 
40  // If position pointer.
41  if(!pPose)
42  throw std::invalid_argument("Invalid pointer: pose of coordinate frame.");
43 
44  // Container for all messages
45  visualization_msgs::MarkerArray msg;
46 
47  // Generate all three axis of the frame.
48  for(unsigned int i = 0; i < 3; i++)
49  msg.markers.push_back(generateAxis(pMarkerId++, i, pPose, pSize));
50 
51  // Publish the markers.
52  pPublisher->publish(msg);
53  }
54 
55  visualization_msgs::Marker CoordinateFrameVisualizer::generateAxis(const unsigned int pMarkerId,
56  const unsigned int pAxis,
57  const boost::shared_ptr<ISM::Pose> pPose,
58  double pSize)
59  {
60  visualization_msgs::Marker msg;
61 
62  // Use the learners frame_id relative to which all locations in this model are given.
63  msg.header.frame_id = getFrameId();
64 
65  // Set timestamp. See the TF tutorials for information on this.
66  msg.header.stamp = ros::Time::now();
67 
68  // Marker namespace are associated to different types of shapes used to represent object location distributions.
69  msg.ns = getNamespace();
70 
71  // Markers with different namespaces but same id represent same object location distribution with different shapes.
72  msg.id = pMarkerId;
73 
74  // The marker type decides on shape of marker and accordingly how marker parameters are interpreted by RViz.
75  // Here we choose a sphere.
76  msg.type = visualization_msgs::Marker::ARROW;
77 
78  // We always want to show a marker by publishing ros messages here.
79  msg.action = visualization_msgs::Marker::ADD;
80 
81  // Arrows should be pretty small. Don't set z here, because we are using start and endpoint here.
82  msg.scale.x = msg.scale.y = 0.01 * pSize;
83 
84  // Create the starting point.
85  geometry_msgs::Point start;
86  start.x = 0;
87  start.y = 0;
88  start.z = 0;
89  msg.points.push_back(start);
90 
91  // Create the point to point to ;).
92  double length = 0.05 * pSize;
93  geometry_msgs::Point stop;
94  stop.x = ((pAxis == 0) ? 1.0 : 0.0) * length;
95  stop.y = ((pAxis - 1 == 0) ? 1.0 : 0.0) * length;
96  stop.z = ((pAxis - 2 == 0) ? 1.0 : 0.0) * length;
97  msg.points.push_back(stop);
98 
99  // Set position and orientation.
100  Eigen::Vector3d position = pPose->point->getEigen();
101  msg.pose.position.x = position[0];
102  msg.pose.position.y = position[1];
103  msg.pose.position.z = position[2];
104 
105  Eigen::Quaternion<double> orientation = pPose->quat->getEigen();
106  msg.pose.orientation.w = orientation.w();
107  msg.pose.orientation.x = orientation.x();
108  msg.pose.orientation.y = orientation.y();
109  msg.pose.orientation.z = orientation.z();
110 
111  // The color depends on the axis number. The color codes RGB stand for the axis XYZ.
112  msg.color.r = (pAxis == 0) ? 1.0 : 0.0;
113  msg.color.g = (pAxis - 1 == 0) ? 1.0 : 0.0;
114  msg.color.b = (pAxis - 2 == 0) ? 1.0 : 0.0;
115  msg.color.a = 1.0;
116 
117  // Return the message.
118  return msg;
119  }
120 
121 }
void publishFrame(boost::shared_ptr< ros::Publisher > pPublisher, unsigned int &pMarkerId, const boost::shared_ptr< ISM::Pose > pPose, double pSize)
visualization_msgs::Marker generateAxis(const unsigned int pMarkerId, const unsigned int pAxis, const boost::shared_ptr< ISM::Pose > pPose, double pSize)
static Time now()
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)


asr_psm_visualizations
Author(s): Gehrung Joachim, Meißner Pascal
autogenerated on Sat Nov 9 2019 03:49:12