60 std::replace(ns.begin(), ns.end(),
'/',
'_');
76 catch (
const std::exception& ex)
78 ROS_FATAL(
"Exception during destruction of Visualization: %s", ex.what());
82 ROS_FATAL(
"Exception during destruction of Visualization");
90 i.action = visualization_msgs::Marker::DELETE;
100 visualization_msgs::Marker marker;
101 marker.header.stamp = timestamp;
102 marker.header.frame_id =
tf_prefix_ + plane.pose_frame;
106 marker.type = visualization_msgs::Marker::CYLINDER;
107 marker.action = visualization_msgs::Marker::ADD;
108 marker.scale.x = 1.0;
109 marker.scale.y = 1.0;
110 marker.scale.z = 0.001;
111 marker.pose.position.x = -plane.normal.x * plane.distance;
112 marker.pose.position.y = -plane.normal.y * plane.distance;
113 marker.pose.position.z = -plane.normal.z * plane.distance;
118 tf2::Quaternion q(-plane.normal.y, plane.normal.x, 0, plane.normal.z + 1);
121 marker.color.a = 0.5;
122 marker.color.r = 0.0;
123 marker.color.g = 1.0;
124 marker.color.b = 0.0;
129 inline geometry_msgs::Transform
toRosTf(
const geometry_msgs::Pose& pose)
131 geometry_msgs::Transform msg;
132 msg.translation.x = pose.position.x;
133 msg.translation.y = pose.position.y;
134 msg.translation.z = pose.position.z;
135 msg.rotation.x = pose.orientation.x;
136 msg.rotation.y = pose.orientation.y;
137 msg.rotation.z = pose.orientation.z;
138 msg.rotation.w = pose.orientation.w;
144 for (
const auto& instance : instances)
146 geometry_msgs::TransformStamped tf;
147 tf.transform =
toRosTf(instance.pose);
148 tf.header.frame_id =
tf_prefix_ + instance.pose_frame;
149 tf.child_frame_id =
tf_prefix_ + instance.object_id +
"_" + instance.id;
150 tf.header.stamp = instance.timestamp;
void publish(const boost::shared_ptr< M > &message) const
void visInstances(const std::vector< Instance > &instances)
visualization_msgs::MarkerArray markers_
Visualizer(ros::NodeHandle &nh)
tf2_ros::TransformBroadcaster tfb_
ROSCPP_DECL const std::string & getNamespace()
static std::string make_tf_prefix(const ros::NodeHandle &nh)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher marker_pub_
void convert(const A &a, B &b)
void visBasePlane(const EstimatedPlane &plane, const ros::Time timestamp)
geometry_msgs::Transform toRosTf(const geometry_msgs::Pose &pose)