robowflex::IO::RVIZHelper Class Reference

RVIZ visualization helper. See Live Visualization with RViz for more information. More...

#include <visualization.h>

Public Member Functions
	RVIZHelper (const RobotConstPtr &robot, const std::string &name="robowflex")
	Constructor. All parameters are placed under /name. This helper puts trajectories under /name/trajectory, scenes under /name/scene, and other markers under /name/markers. The robot description is placed under /name/robot_description. More...
Trajectories
void	updateTrajectory (const planning_interface::MotionPlanResponse &response)
	Updates the trajectory being visualized. More...
void	updateTrajectory (const Trajectory &trajectory)
	Updates the trajectory being visualized. More...
void	updateTrajectory (const robot_trajectory::RobotTrajectoryPtr &trajectory)
	Updates the trajectory being visualized. More...
void	updateTrajectory (const moveit_msgs::RobotTrajectory &traj, const moveit::core::RobotState &start)
	Updates the trajectory being visualized. More...
void	updateTrajectories (const std::vector< robot_trajectory::RobotTrajectoryPtr > &trajectories)
	Updates the trajectory being visualized to a list of trajectories. More...
void	updateTrajectories (const std::vector< planning_interface::MotionPlanResponse > &responses)
	Updates the trajectory being visualized to a list of trajectories. More...
void	updateTrajectories (const std::vector< TrajectoryPtr > &trajectories)
	Updates the trajectory being visualized to a list of trajectories. More...
States
void	visualizeState (const robot_state::RobotStatePtr &state)
	Visualizes a robot state. More...
void	visualizeState (const std::vector< double > &state)
	Visualizes a robot state. More...
void	visualizeCurrentState ()
	Visualize the current state of the robot. More...
Scenes
void	removeScene ()
	Removes the scene being visualized. More...
void	updateScene (const SceneConstPtr &scene)
	Updates the scene being visualized. More...
void	updatePCD (const sensor_msgs::PointCloud2 &msg)
	Updates the pointcloud being visualized. More...
Markers
void	addMarker (const visualization_msgs::Marker &marker, const std::string &name="")
	Add a marker message (under the name "name") to the scene. More...
void	addMarker (double x, double y, double z, const std::string &name="")
	Add a spherical marker (under the name "name") to the scene. More...
void	addMarker (const Eigen::Vector3d &point, const std::string &name="")
	Add a point (spherical) marker to the scene. More...
void	addTransformMarker (const std::string &name, const std::string &base_frame, const RobotPose &pose, double scale=1)
	Add a transform marker to the managed list of markers. Displayed after an updateMarkers() call. More...
void	addGeometryMarker (const std::string &name, const GeometryConstPtr &geometry, const std::string &base_frame, const RobotPose &pose, const Eigen::Vector4d &color=color::WHITE)
	Add a marker to the managed list of markers. Displayed after an updateMarkers() call. More...
void	addArrowMarker (const std::string &name, const std::string &base_frame, const RobotPose &pose, const Eigen::Vector4d &color, const Eigen::Vector3d &scale)
	Adds an arrow marker to the managed list of markers. Displayed after an updateMarkers() call. More...
void	addTextMarker (const std::string &name, const std::string &text, const std::string &base_frame, const RobotPose &pose, double height, const Eigen::Vector4d &color=color::WHITE)
	Adds a text marker to the managed list of markers. Displayed after updateMarkers(). More...
void	addLineMarker (const std::string &name, const std::vector< Eigen::Vector3d > &points, const std::vector< Eigen::Vector4d > &colors, double scale)
	Add a set of lines as markers. More...
void	addGoalMarker (const std::string &name, const MotionRequestBuilder &request)
	Adds the current goal of the motion request builder as a set of markers in the marker array. More...
void	addGoalMarker (const std::string &name, const moveit_msgs::MotionPlanRequest &request)
	Adds the current goal of the motion plan request as a set of markers in the marker array. More...
void	removeAllMarkers ()
	Removes all markers that were added through addMarker(). More...
void	removeMarker (const std::string &name)
	Removes a marker that was added through addMarker(). More...
void	updateMarkers ()
	Displays the managed list of markers. Keeps track of whether markers have already been displayed and simply need an update, and removes markers removed by removeMarker(). More...

Private Member Functions
void	fillMarker (visualization_msgs::Marker &marker, const std::string &base_frame, const RobotPose &pose, const Eigen::Vector4d &color, const Eigen::Vector3d &scale) const
	Fills a marker in with some common default information. More...

Private Attributes
RobotConstPtr	robot_
	Robot being visualized. More...
ros::NodeHandle	nh_
	Handle for publishing. More...
ros::Publisher	marker_pub_
	Marker publisher. More...
ros::Publisher	trajectory_pub_
	Trajectory publisher. More...
ros::Publisher	scene_pub_
	Scene publisher. More...
ros::Publisher	pcd_pub_
	Pointcloud publisher. More...
ros::Publisher	state_pub_
	State publisher. More...
std::multimap< std::string, visualization_msgs::Marker >	markers_
	Markers to publish. More...

Detailed Description

RVIZ visualization helper. See Live Visualization with RViz for more information.

Definition at line 37 of file visualization.h.

Constructor & Destructor Documentation

RVIZHelper()

IO::RVIZHelper::RVIZHelper	(	const RobotConstPtr &	robot,
		const std::string &	name = "robowflex"
	)

Constructor. All parameters are placed under /name. This helper puts trajectories under /name/trajectory, scenes under /name/scene, and other markers under /name/markers. The robot description is placed under /name/robot_description.

Parameters

[in]	robot	Robot to model with this helper.
[in]	name	Namespace to use.

Definition at line 39 of file visualization.cpp.

  : robot_(robot), nh_("/" + name)
{
    std::string description;
    robot_->getHandlerConst().getParam(Robot::ROBOT_DESCRIPTION, description);
    std::string semantic;
    robot_->getHandlerConst().getParam(Robot::ROBOT_DESCRIPTION + Robot::ROBOT_SEMANTIC, semantic);
 
    nh_.setParam(Robot::ROBOT_DESCRIPTION, description);
    nh_.setParam(Robot::ROBOT_DESCRIPTION + Robot::ROBOT_SEMANTIC, semantic);
 
    trajectory_pub_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("trajectory", 1);
    state_pub_ = nh_.advertise<moveit_msgs::DisplayRobotState>("state", 1);
    scene_pub_ = nh_.advertise<moveit_msgs::PlanningScene>("scene", 1);
    pcd_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("pcd", 1);
    marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/visualization_marker_array", 100);
}

Member Function Documentation

addArrowMarker()

void IO::RVIZHelper::addArrowMarker	(	const std::string &	name,
		const std::string &	base_frame,
		const RobotPose &	pose,
		const Eigen::Vector4d &	color,
		const Eigen::Vector3d &	scale
	)

Adds an arrow marker to the managed list of markers. Displayed after an updateMarkers() call.

Parameters

[in]	name	Name of the marker.
[in]	base_frame	Base frame of the pose of the marker.
[in]	pose	Pose of the marker.
[in]	color	Color of the marker.
[in]	scale	The scale of the marker.

Definition at line 204 of file visualization.cpp.

{
    visualization_msgs::Marker marker;
    fillMarker(marker, base_frame, pose, color, scale);
 
    marker.type = visualization_msgs::Marker::ARROW;
 
    addMarker(marker, name);
}

addGeometryMarker()

void IO::RVIZHelper::addGeometryMarker	(	const std::string &	name,
		const GeometryConstPtr &	geometry,
		const std::string &	base_frame,
		const RobotPose &	pose,
		const Eigen::Vector4d &	color = color::WHITE
	)

Add a marker to the managed list of markers. Displayed after an updateMarkers() call.

Parameters

[in]	name	Name of the marker.
[in]	geometry	Geometry of the marker to create.
[in]	base_frame	Base frame of the pose of the marker.
[in]	pose	Pose of the marker.
[in]	color	Color of the marker.

Definition at line 271 of file visualization.cpp.

{
    visualization_msgs::Marker marker;
 
    auto scale = geometry->getDimensions();
    switch (geometry->getType())
    {
        case Geometry::ShapeType::BOX:
            marker.type = visualization_msgs::Marker::CUBE;
            break;
        case Geometry::ShapeType::SPHERE:
            marker.type = visualization_msgs::Marker::SPHERE;
            scale[1] = scale[2] = scale[0];  // Copy radius to other dimensions
            break;
        case Geometry::ShapeType::CYLINDER:
            marker.type = visualization_msgs::Marker::CYLINDER;
            {
                auto scale2 = scale;
                scale[0] = scale[1] = 2 * scale2[0];  // Copy radius to first two (x & y)
                scale[2] = scale2[1];
            }
            break;
        case Geometry::ShapeType::MESH:
            scale[0] = scale[1] = scale[2] = 1;  // Meshes are unscalable.
            if (!geometry->getResource().empty())
            {
                marker.type = visualization_msgs::Marker::MESH_RESOURCE;
                marker.mesh_resource = geometry->getResource();
                marker.mesh_use_embedded_materials = true;
            }
            else if (!geometry->getVertices().empty())
            {
                auto msg = geometry->getMeshMsg();
                marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
                for (std::size_t i = 0; i < msg.triangles.size(); ++i)
                {
                    marker.points.push_back(msg.vertices[msg.triangles[i].vertex_indices[0]]);
                    marker.points.push_back(msg.vertices[msg.triangles[i].vertex_indices[1]]);
                    marker.points.push_back(msg.vertices[msg.triangles[i].vertex_indices[2]]);
                }
            }
            break;
 
        default:
            RBX_ERROR("Unsupported geometry for marker.");
            return;
    };
 
    fillMarker(marker, base_frame, pose, color, scale);
 
    addMarker(marker, name);
}

addGoalMarker() [1/2]

void IO::RVIZHelper::addGoalMarker	(	const std::string &	name,
		const MotionRequestBuilder &	request
	)

Adds the current goal of the motion request builder as a set of markers in the marker array.

Parameters

[in]	name	Name of the marker(s).
[in]	request	Request to add goal of as a marker.

Definition at line 421 of file visualization.cpp.

{
    addGoalMarker(name, request.getRequestConst());
}

addGoalMarker() [2/2]

void IO::RVIZHelper::addGoalMarker	(	const std::string &	name,
		const moveit_msgs::MotionPlanRequest &	request
	)

Adds the current goal of the motion plan request as a set of markers in the marker array.

Parameters

[in]	name	Name of the marker(s).
[in]	request	Request to add goal of as a marker.

Definition at line 326 of file visualization.cpp.

{
    const auto &goals = request.goal_constraints;
 
    // Iterate over each goal (an "or"-ing together of different constraints)
    for (const auto &goal : goals)
    {
        auto color = getRandomColor();  // Use the same color for all elements of this goal
        color[3] = 0.7;                 // Make slightly transparent
 
        for (const auto &pg : goal.position_constraints)
        {
            const auto &pname = pg.link_name;
            const auto &base_frame = pg.header.frame_id;
 
            // Get global transform of position constraint
            RobotPose pose;
            if (base_frame == robot_->getModelConst()->getModelFrame())
            {
                pose.setIdentity();
            }
            else
            {
                pose = robot_->getLinkTF(pg.header.frame_id);
            }
            pose.translate(TF::vectorMsgToEigen(pg.target_point_offset));
 
            // Iterate over all position primitives and their poses
            for (const auto &primitive :
                 boost::combine(pg.constraint_region.primitives, pg.constraint_region.primitive_poses))
            {
                shape_msgs::SolidPrimitive solid;
                geometry_msgs::Pose solid_pose;
                boost::tie(solid, solid_pose) = primitive;
 
                // Compute transform of bounding volume
                auto frame = pose * TF::poseMsgToEigen(solid_pose);
 
                // Add geometry marker associated with this solid primitive
                addGeometryMarker(name, Geometry::makeSolidPrimitive(solid), base_frame, frame, color);
 
                // Iterate over all orientation constraints for the same link as the
                // position constraint
                for (const auto &og : goal.orientation_constraints)
                {
                    const auto &oname = og.link_name;
                    if (oname != pname)
                        continue;
 
                    auto q = TF::quaternionMsgToEigen(og.orientation);
 
                    // Arrow display frame.
                    RobotPose qframe = RobotPose::Identity();
                    qframe.translate(frame.translation());  // Place arrows at the origin
                    // of the position volume
 
                    Eigen::Vector3d scale = {0.1, 0.008, 0.003};  // A nice default size of arrow
 
                    // Display primary orientation slightly larger
                    addArrowMarker(name, base_frame, qframe * q, color, 1.5 * scale);
 
                    // Zip together tolerances and axes, and iterate over them to display
                    // orientation bounds
                    const auto tolerances = {og.absolute_x_axis_tolerance,  //
                                             og.absolute_y_axis_tolerance,  //
                                             og.absolute_z_axis_tolerance};
                    const auto axes = {Eigen::Vector3d::UnitX(),  //
                                       Eigen::Vector3d::UnitY(),  //
                                       Eigen::Vector3d::UnitZ()};
                    for (const auto &angles : boost::combine(tolerances, axes))
                    {
                        double value;
                        Eigen::Vector3d axis;
                        boost::tie(value, axis) = angles;
 
                        // Show boundaries of tolerances as smaller arrows.
                        auto q1 = TF::offsetOrientation(q, axis, value);
                        addArrowMarker(name, base_frame, qframe * q1, color, scale);
 
                        auto q2 = TF::offsetOrientation(q, axis, -value);
                        addArrowMarker(name, base_frame, qframe * q2, color, scale);
                    }
                }
            }
 
            // TODO: Implement
            // for (const auto &mesh :
            //      boost::combine(pg.constraint_region.meshes,
            //      pg.constraint_region.mesh_poses))
            // {
            // }
        }
    }
}

addLineMarker()

void IO::RVIZHelper::addLineMarker	(	const std::string &	name,
		const std::vector< Eigen::Vector3d > &	points,
		const std::vector< Eigen::Vector4d > &	colors,
		double	scale
	)

Add a set of lines as markers.

Parameters

[in]	name	Name of the marker.
[in]	points	Pair-wise list of points to add as lines. (eg., 0-1, 2-3, ...)
[in]	colors	List of colors for each point.
[in]	scale	Scale of the marker.

Definition at line 229 of file visualization.cpp.

{
    visualization_msgs::Marker marker;
 
    fillMarker(marker,                 //
               "map",                  //
               RobotPose::Identity(),  //
               getRandomColor(),       //
               Eigen::Vector3d{scale, 1, 1});
 
    marker.type = visualization_msgs::Marker::LINE_LIST;
 
    if (points.size() != colors.size())
        throw Exception(1, "Mismatch between points and colors sizes!");
 
    for (std::size_t i = 0; i < points.size(); ++i)
    {
        marker.points.emplace_back(TF::pointEigenToMsg(points[i]));
        std_msgs::ColorRGBA mcolor;
        mcolor.r = colors[i][0];
        mcolor.g = colors[i][1];
        mcolor.b = colors[i][2];
        mcolor.a = colors[i][3];
        marker.colors.emplace_back(mcolor);
    }
 
    addMarker(marker, name);
}

addMarker() [1/3]

void IO::RVIZHelper::addMarker	(	const Eigen::Vector3d &	point,
		const std::string &	name = ""
	)

Add a point (spherical) marker to the scene.

Parameters

[in]	point	The x,y,z coordinates as a vector
[in]	name	Name of marker.

Definition at line 462 of file visualization.cpp.

{
    addMarker(point.x(), point.y(), point.z(), name);
}

addMarker() [2/3]

void IO::RVIZHelper::addMarker	(	const visualization_msgs::Marker &	marker,
		const std::string &	name = ""
	)

Add a marker message (under the name "name") to the scene.

Parameters

[in]	marker	A marker message.
[in]	name	Name of marker.

Definition at line 440 of file visualization.cpp.

{
    markers_.emplace(name, marker);
}

addMarker() [3/3]

void IO::RVIZHelper::addMarker	(	double	x,
		double	y,
		double	z,
		const std::string &	name = ""
	)

Add a spherical marker (under the name "name") to the scene.

Parameters

[in]	x	The x coordinate of the sphere.
[in]	y	The y coordinate of the sphere.
[in]	z	The z coordinate of the sphere.
[in]	name	Name of marker.

Definition at line 445 of file visualization.cpp.

{
    visualization_msgs::Marker marker;
    const std::string &base_frame = "map";
 
    const auto &pose = TF::createPoseXYZ(x, y, z);
 
    const auto &scale = Eigen::Vector3d{0.05, 0.05, 0.05};
    const auto &color = getRandomColor();
 
    fillMarker(marker, base_frame, pose, color, scale);
 
    marker.type = visualization_msgs::Marker::SPHERE;
 
    addMarker(marker, name);
}

addTextMarker()

void IO::RVIZHelper::addTextMarker	(	const std::string &	name,
		const std::string &	text,
		const std::string &	base_frame,
		const RobotPose &	pose,
		double	height,
		const Eigen::Vector4d &	color = color::WHITE
	)

Adds a text marker to the managed list of markers. Displayed after updateMarkers().

Parameters

[in]	name	Name of the marker.
[in]	text	The text to display.
[in]	base_frame	Base frame of the pose of the marker.
[in]	pose	Pose of the marker.
[in]	height	The height of the text.
[in]	color	Color of the marker.

Definition at line 216 of file visualization.cpp.

{
    visualization_msgs::Marker marker;
    fillMarker(marker, base_frame, pose, color, {0, 0, height});
 
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker.text = text;
 
    addMarker(marker, name);
}

addTransformMarker()

void IO::RVIZHelper::addTransformMarker	(	const std::string &	name,
		const std::string &	base_frame,
		const RobotPose &	pose,
		double	scale = 1
	)

Add a transform marker to the managed list of markers. Displayed after an updateMarkers() call.

Parameters

[in]	name	Name of the marker.
[in]	base_frame	Base frame of the pose of the marker.
[in]	pose	Pose of the transform.
[in]	scale	Scale factor that controls the size of the frame marker.

Definition at line 259 of file visualization.cpp.

{
    const auto &arrow_size = Eigen::Vector3d{0.1, 0.008, 0.003};  // A nice default size of arrow
    const auto &z_rot90 = TF::createPoseXYZ(0, 0, 0, 0, 0, constants::half_pi);
    const auto &y_rot90 = TF::createPoseXYZ(0, 0, 0, 0, -constants::half_pi, 0);
 
    addArrowMarker(name + "X", base_frame, pose, color::RED, scale * arrow_size);
    addArrowMarker(name + "Y", base_frame, pose * z_rot90, color::GREEN, scale * arrow_size);
    addArrowMarker(name + "Z", base_frame, pose * y_rot90, color::BLUE, scale * arrow_size);
}

fillMarker()

void IO::RVIZHelper::fillMarker ( visualization_msgs::Marker &  marker, const std::string &  base_frame, const RobotPose &  pose, const Eigen::Vector4d &  color, const Eigen::Vector3d &  scale  ) const

private

Fills a marker in with some common default information.

Parameters

[out]	marker	Marker to fill.
[in]	base_frame	Base frame of the pose of the marker.
[in]	pose	Pose of the marker.
[in]	color	Color of the marker.
[in]	scale	The scale of the marker.

Definition at line 183 of file visualization.cpp.

{
    marker.header.frame_id = base_frame;
    marker.frame_locked = true;
 
    marker.header.stamp = ros::Time().now();
    marker.ns = "/robowflex";
    marker.id = markers_.size();
 
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose = TF::poseEigenToMsg(pose);
    marker.scale = TF::vectorEigenToMsg(scale);
 
    marker.color.r = color[0];
    marker.color.g = color[1];
    marker.color.b = color[2];
    marker.color.a = color[3];
}

removeAllMarkers()

void IO::RVIZHelper::removeAllMarkers

(

)

Removes all markers that were added through addMarker().

Definition at line 426 of file visualization.cpp.

{
    for (auto &marker : markers_)
        marker.second.action = visualization_msgs::Marker::DELETE;
}

removeMarker()

void IO::RVIZHelper::removeMarker

(

const std::string &

name

)

Removes a marker that was added through addMarker().

Parameters

[in]

name

The name of the marker to remove.

Definition at line 432 of file visualization.cpp.

{
    auto markers = markers_.equal_range(name);
 
    for (auto it = markers.first; it != markers.second; ++it)
        it->second.action = visualization_msgs::Marker::DELETE;
}

removeScene()

void IO::RVIZHelper::removeScene

(

)

Removes the scene being visualized.

Definition at line 467 of file visualization.cpp.

{
    updateScene(nullptr);
}

updateMarkers()

void IO::RVIZHelper::updateMarkers

(

)

Displays the managed list of markers. Keeps track of whether markers have already been displayed and simply need an update, and removes markers removed by removeMarker().

Definition at line 507 of file visualization.cpp.

{
    visualization_msgs::MarkerArray msg;
 
    std::vector<std::string> remove;
    for (auto &marker : markers_)
    {
        msg.markers.emplace_back(marker.second);
 
        if (marker.second.action == visualization_msgs::Marker::ADD)
            marker.second.action = visualization_msgs::Marker::MODIFY;
 
        else if (marker.second.action == visualization_msgs::Marker::DELETE)
            remove.push_back(marker.first);
    }
 
    if (marker_pub_.getNumSubscribers() < 1)
    {
        RBX_INFO("Waiting for MarkerArray subscribers...");
 
        ros::WallDuration pause(0.1);
        while (marker_pub_.getNumSubscribers() < 1)
            pause.sleep();
    }
 
    marker_pub_.publish(msg);
 
    for (auto &marker : remove)
        markers_.erase(markers_.find(marker));
}

updatePCD()

void IO::RVIZHelper::updatePCD

(

const sensor_msgs::PointCloud2 &

msg

)

Updates the pointcloud being visualized.

Parameters

[in]

msg

Pointcloud msg to visualize.

Definition at line 493 of file visualization.cpp.

{
    if (pcd_pub_.getNumSubscribers() < 1)
    {
        RBX_INFO("Waiting for pcd subscribers...");
 
        ros::WallDuration pause(0.1);
        while (pcd_pub_.getNumSubscribers() < 1)
            pause.sleep();
    }
 
    pcd_pub_.publish(msg);
}

updateScene()

void IO::RVIZHelper::updateScene

(

const SceneConstPtr &

scene

)

Updates the scene being visualized.

Parameters

[in]

scene

Scene to visualize. If null, removes scene by publishing empty message.

Definition at line 472 of file visualization.cpp.

{
    if (scene_pub_.getNumSubscribers() < 1)
    {
        RBX_INFO("Waiting for Scene subscribers...");
 
        ros::WallDuration pause(0.1);
        while (scene_pub_.getNumSubscribers() < 1)
            pause.sleep();
    }
 
    moveit_msgs::PlanningScene to_pub;
    if (scene != nullptr)
    {
        to_pub = scene->getMessage();
        to_pub.is_diff = true;
    }
 
    scene_pub_.publish(to_pub);
}

updateTrajectories() [1/3]

void IO::RVIZHelper::updateTrajectories

(

const std::vector< planning_interface::MotionPlanResponse > &

responses

)

Updates the trajectory being visualized to a list of trajectories.

Parameters

[in]

responses

Planning responses to visualize.

Definition at line 127 of file visualization.cpp.

{
    auto moveit_trajectories = std::vector<robot_trajectory::RobotTrajectoryPtr>();
 
    for (const auto &response : responses)
        if (response.error_code_.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
            moveit_trajectories.push_back(response.trajectory_);
 
    updateTrajectories(moveit_trajectories);
}

updateTrajectories() [2/3]

void IO::RVIZHelper::updateTrajectories

(

const std::vector< robot_trajectory::RobotTrajectoryPtr > &

trajectories

)

Updates the trajectory being visualized to a list of trajectories.

Parameters

[in]

trajectories

Vector of MoveIt! robot trajectories to visualize.

Definition at line 96 of file visualization.cpp.

{
    moveit_msgs::DisplayTrajectory out;
    out.model_id = robot_->getModelName();
 
    bool set = false;
    for (const auto &traj : trajectories)
    {
        if (!set)
        {
            moveit::core::robotStateToRobotStateMsg(traj->getFirstWayPoint(), out.trajectory_start);
            set = true;
        }
 
        moveit_msgs::RobotTrajectory msg;
        traj->getRobotTrajectoryMsg(msg);
        out.trajectory.push_back(msg);
    }
 
    if (trajectory_pub_.getNumSubscribers() < 1)
    {
        RBX_INFO("Waiting for Trajectory subscribers...");
 
        ros::WallDuration pause(0.1);
        while (trajectory_pub_.getNumSubscribers() < 1)
            pause.sleep();
    }
 
    trajectory_pub_.publish(out);
}

updateTrajectories() [3/3]

void IO::RVIZHelper::updateTrajectories

(

const std::vector< TrajectoryPtr > &

trajectories

)

Updates the trajectory being visualized to a list of trajectories.

Parameters

[in]

responses

Vector of robowflex trajectories to visualize.

Definition at line 138 of file visualization.cpp.

{
    auto moveit_trajectories = std::vector<robot_trajectory::RobotTrajectoryPtr>();
 
    for (const auto &traj : trajectories)
        moveit_trajectories.push_back(traj->getTrajectory());
 
    updateTrajectories(moveit_trajectories);
}

updateTrajectory() [1/4]

void IO::RVIZHelper::updateTrajectory	(	const moveit_msgs::RobotTrajectory &	traj,
		const moveit::core::RobotState &	start
	)

Updates the trajectory being visualized.

Parameters

[in]	traj	RobotTrajectory to visualize.
[in]	start	base_state to copy values for other joints.

Definition at line 75 of file visualization.cpp.

{
    moveit_msgs::DisplayTrajectory out;
 
    out.model_id = robot_->getModelName();
    out.trajectory.push_back(traj);
    moveit::core::robotStateToRobotStateMsg(start, out.trajectory_start);
 
    if (trajectory_pub_.getNumSubscribers() < 1)
    {
        RBX_INFO("Waiting for Trajectory subscribers...");
 
        ros::WallDuration pause(0.1);
        while (trajectory_pub_.getNumSubscribers() < 1)
            pause.sleep();
    }
 
    trajectory_pub_.publish(out);
}

updateTrajectory() [2/4]

void IO::RVIZHelper::updateTrajectory

(

const planning_interface::MotionPlanResponse &

response

)

Updates the trajectory being visualized.

Parameters

[in]

response

Planning response to visualize.

Definition at line 57 of file visualization.cpp.

{
    updateTrajectory(response.trajectory_);
}

updateTrajectory() [3/4]

void IO::RVIZHelper::updateTrajectory

(

const robot_trajectory::RobotTrajectoryPtr &

trajectory

)

Updates the trajectory being visualized.

Parameters

[in]

trajectory

Trajectory to visualize.

Definition at line 67 of file visualization.cpp.

{
    moveit_msgs::RobotTrajectory msg;
    trajectory->getRobotTrajectoryMsg(msg);
 
    updateTrajectory(msg, trajectory->getFirstWayPoint());
}

updateTrajectory() [4/4]

void IO::RVIZHelper::updateTrajectory

(

const Trajectory &

trajectory

)

Updates the trajectory being visualized.

Parameters

[in]

trajectory

Trajectory to visualize.

Definition at line 62 of file visualization.cpp.

{
    updateTrajectory(trajectory.getTrajectoryConst());
}

visualizeCurrentState()

void IO::RVIZHelper::visualizeCurrentState

(

)

Visualize the current state of the robot.

Definition at line 178 of file visualization.cpp.

{
    visualizeState(robot_->getScratchStateConst());
}

visualizeState() [1/2]

void IO::RVIZHelper::visualizeState

(

const robot_state::RobotStatePtr &

state

)

Visualizes a robot state.

Parameters

[in]

state

The state of the robot to be visualized.

Definition at line 148 of file visualization.cpp.

{
    if (state_pub_.getNumSubscribers() < 1)
    {
        RBX_INFO("Waiting for State subscribers...");
 
        ros::WallDuration pause(0.1);
        while (state_pub_.getNumSubscribers() < 1)
            pause.sleep();
    }
 
    moveit_msgs::DisplayRobotState out;
    moveit::core::robotStateToRobotStateMsg(*state, out.state);
 
    state_pub_.publish(out);
}

visualizeState() [2/2]

void IO::RVIZHelper::visualizeState

(

const std::vector< double > &

state

)

Visualizes a robot state.

Parameters

[in]

state

The state of the robot to be visualized.

Definition at line 165 of file visualization.cpp.

{
    auto state = robot_->allocState();
 
    sensor_msgs::JointState joint_state;
    joint_state.name = robot_->getJointNames();
    joint_state.position = state_vec;
 
    moveit::core::jointStateToRobotState(joint_state, *state);
 
    visualizeState(state);
}

Member Data Documentation

marker_pub_

ros::Publisher robowflex::IO::RVIZHelper::marker_pub_

private

Marker publisher.

Definition at line 250 of file visualization.h.

markers_

std::multimap<std::string, visualization_msgs::Marker> robowflex::IO::RVIZHelper::markers_

private

Markers to publish.

Definition at line 256 of file visualization.h.

nh_

ros::NodeHandle robowflex::IO::RVIZHelper::nh_

private

Handle for publishing.

Definition at line 249 of file visualization.h.

pcd_pub_

ros::Publisher robowflex::IO::RVIZHelper::pcd_pub_

private

Pointcloud publisher.

Definition at line 253 of file visualization.h.

robot_

RobotConstPtr robowflex::IO::RVIZHelper::robot_

private

Robot being visualized.

Definition at line 248 of file visualization.h.

scene_pub_

ros::Publisher robowflex::IO::RVIZHelper::scene_pub_

private

Scene publisher.

Definition at line 252 of file visualization.h.

state_pub_

ros::Publisher robowflex::IO::RVIZHelper::state_pub_

private

State publisher.

Definition at line 254 of file visualization.h.

trajectory_pub_

ros::Publisher robowflex::IO::RVIZHelper::trajectory_pub_

private

Trajectory publisher.

Definition at line 251 of file visualization.h.

---

The documentation for this class was generated from the following files:

• visualization.h
• visualization.cpp