Component for visualizing trajectories as RViz markers. More...

#include <cp_trajectory_visualizer.hpp>

Inheritance diagram for cl_moveit2z::CpTrajectoryVisualizer:

Collaboration diagram for cl_moveit2z::CpTrajectoryVisualizer:

Public Member Functions
	CpTrajectoryVisualizer (double publishRate=10.0)
	Constructor.

virtual	~CpTrajectoryVisualizer ()=default

void	onInitialize () override
	Initialize the marker publisher.

void	update () override
	Periodic update - publishes markers if enabled.

void	setTrajectory (const std::vector< geometry_msgs::msg::PoseStamped > &poses, const std::string &ns="trajectory")
	Set trajectory to visualize.

void	clearMarkers ()
	Clear all markers.

void	setColor (float r, float g, float b, float a=1.0)
	Set marker color (RGBA values 0.0-1.0)

void	setScale (double scale)
	Set marker scale (sphere diameter in meters)

void	setEnabled (bool enabled)
	Enable/disable marker publishing.

bool	isEnabled () const
	Check if visualizer is enabled.

Public Member Functions inherited from smacc2::ISmaccComponent
	ISmaccComponent ()

virtual	~ISmaccComponent ()

virtual std::string	getName () const

Public Member Functions inherited from smacc2::ISmaccUpdatable
	ISmaccUpdatable ()

	ISmaccUpdatable (rclcpp::Duration duration)

void	executeUpdate (rclcpp::Node::SharedPtr node)

void	setUpdatePeriod (rclcpp::Duration duration)

Private Attributes
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	markersPub_

visualization_msgs::msg::MarkerArray	markers_

std::mutex	markersMutex_

bool	markersReady_ = false

bool	enabled_ = false

double	markerScale_ = 0.02

float	markerColor_ [4] = {0.0f, 1.0f, 0.0f, 0.8f}

double	publishRate_

rclcpp::Time	lastPublishTime_

Additional Inherited Members
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename TOrthogonal , typename TClient >
void	onComponentInitialization ()

template<typename EventType >
void	postEvent (const EventType &ev)

template<typename EventType >
void	postEvent ()

template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()

template<typename TOrthogonal , typename TSourceObject >
void	onStateOrthogonalAllocation ()

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist)

template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, ComponentRequirement requirementType=ComponentRequirement::SOFT)

template<typename TClient >
void	requiresClient (TClient *&requiredClientStorage)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingComponent (TArgs... targs)

template<typename SmaccComponentType , typename TOrthogonal , typename TClient , typename... TArgs>
SmaccComponentType *	createSiblingNamedComponent (std::string name, TArgs... targs)

rclcpp::Node::SharedPtr	getNode ()

rclcpp::Logger	getLogger () const

ISmaccStateMachine *	getStateMachine ()

Protected Member Functions inherited from smacc2::ISmaccUpdatable
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_

ISmaccClient *	owner_

Detailed Description

Component for visualizing trajectories as RViz markers.

This component publishes trajectory paths as visualization markers that can be viewed in RViz. It follows the ISmaccUpdatable pattern for periodic publishing.

Pattern: Publisher + ISmaccUpdatable (similar to nav2z_client's CpWaypointsVisualizer)

Definition at line 44 of file cp_trajectory_visualizer.hpp.

Constructor & Destructor Documentation

◆ CpTrajectoryVisualizer()

cl_moveit2z::CpTrajectoryVisualizer::CpTrajectoryVisualizer ( double publishRate = 10.0 )

inline

Constructor.

Parameters

publishRate Publishing rate (default: 10Hz)

Definition at line 52 of file cp_trajectory_visualizer.hpp.

52: publishRate_(publishRate), enabled_(false) {}

cl_moveit2z::CpTrajectoryVisualizer::enabled_

bool enabled_

Definition cp_trajectory_visualizer.hpp:234

cl_moveit2z::CpTrajectoryVisualizer::publishRate_

double publishRate_

Definition cp_trajectory_visualizer.hpp:241

◆ ~CpTrajectoryVisualizer()

virtual cl_moveit2z::CpTrajectoryVisualizer::~CpTrajectoryVisualizer ( )

virtualdefault

Member Function Documentation

◆ clearMarkers()

void cl_moveit2z::CpTrajectoryVisualizer::clearMarkers ( )

inline

Clear all markers.

Definition at line 176 of file cp_trajectory_visualizer.hpp.

  {
    std::lock_guard<std::mutex> guard(markersMutex_);
 
    RCLCPP_INFO(getLogger(), "[CpTrajectoryVisualizer] Clearing markers");
 
    // Send delete action for all markers
    for (auto & marker : markers_.markers)
    {
      marker.action = visualization_msgs::msg::Marker::DELETE;
    }
 
    if (!markers_.markers.empty())
    {
      markersPub_->publish(markers_);
    }
 
    markers_.markers.clear();
    markersReady_ = false;
    enabled_ = false;
  }

References enabled_, smacc2::ISmaccComponent::getLogger(), markers_, markersMutex_, markersPub_, and markersReady_.

Referenced by cl_moveit2z::CbMoveEndEffectorTrajectory::onExit().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isEnabled()

bool cl_moveit2z::CpTrajectoryVisualizer::isEnabled ( ) const

inline

Check if visualizer is enabled.

Definition at line 226 of file cp_trajectory_visualizer.hpp.

226{ return enabled_; }

References enabled_.

◆ onInitialize()

void cl_moveit2z::CpTrajectoryVisualizer::onInitialize ( )

inlineoverridevirtual

Initialize the marker publisher.

Reimplemented from smacc2::ISmaccComponent.

Definition at line 59 of file cp_trajectory_visualizer.hpp.

  {
    markersPub_ =
      getNode()->create_publisher<visualization_msgs::msg::MarkerArray>("trajectory_markers", 1);
    RCLCPP_INFO(getLogger(), "[CpTrajectoryVisualizer] Initialized");
 
    // Initialize timing for publish rate control
    lastPublishTime_ = getNode()->now();
  }

References smacc2::ISmaccComponent::getLogger(), smacc2::ISmaccComponent::getNode(), lastPublishTime_, and markersPub_.

Here is the call graph for this function:

◆ setColor()

void cl_moveit2z::CpTrajectoryVisualizer::setColor	(	float	r,
		float	g,
		float	b,
		float	a = 1.0 )

inline

Set marker color (RGBA values 0.0-1.0)

Definition at line 201 of file cp_trajectory_visualizer.hpp.

  {
    markerColor_[0] = r;
    markerColor_[1] = g;
    markerColor_[2] = b;
    markerColor_[3] = a;
  }

References markerColor_.

◆ setEnabled()

void cl_moveit2z::CpTrajectoryVisualizer::setEnabled ( bool enabled )

inline

Enable/disable marker publishing.

Definition at line 217 of file cp_trajectory_visualizer.hpp.

  {
    std::lock_guard<std::mutex> guard(markersMutex_);
    enabled_ = enabled;
  }

References enabled_, and markersMutex_.

◆ setScale()

void cl_moveit2z::CpTrajectoryVisualizer::setScale ( double scale )

inline

Set marker scale (sphere diameter in meters)

Definition at line 212 of file cp_trajectory_visualizer.hpp.

212{ markerScale_ = scale; }

cl_moveit2z::CpTrajectoryVisualizer::markerScale_

double markerScale_

Definition cp_trajectory_visualizer.hpp:237

References markerScale_.

◆ setTrajectory()

void cl_moveit2z::CpTrajectoryVisualizer::setTrajectory	(	const std::vector< geometry_msgs::msg::PoseStamped > &	poses,
		const std::string &	ns = "trajectory" )

inline

Set trajectory to visualize.

Parameters

poses	Vector of poses defining the trajectory path
ns	Namespace for the markers (default: "trajectory")

Definition at line 99 of file cp_trajectory_visualizer.hpp.

  {
    std::lock_guard<std::mutex> guard(markersMutex_);
 
    RCLCPP_INFO(
      getLogger(), "[CpTrajectoryVisualizer] Setting trajectory with %lu poses", poses.size());
 
    markers_.markers.clear();
 
    if (poses.empty())
    {
      RCLCPP_WARN(getLogger(), "[CpTrajectoryVisualizer] Empty trajectory provided");
      markersReady_ = false;
      return;
    }
 
    // Create spheres for each pose
    for (size_t i = 0; i < poses.size(); ++i)
    {
      visualization_msgs::msg::Marker marker;
      marker.header = poses[i].header;
      marker.ns = ns;
      marker.id = static_cast<int>(i);
      marker.type = visualization_msgs::msg::Marker::SPHERE;
      marker.action = visualization_msgs::msg::Marker::ADD;
 
      marker.pose = poses[i].pose;
 
      marker.scale.x = markerScale_;
      marker.scale.y = markerScale_;
      marker.scale.z = markerScale_;
 
      marker.color.r = markerColor_[0];
      marker.color.g = markerColor_[1];
      marker.color.b = markerColor_[2];
      marker.color.a = markerColor_[3];
 
      markers_.markers.push_back(marker);
    }
 
    // Create line strip connecting all poses
    if (poses.size() > 1)
    {
      visualization_msgs::msg::Marker lineMarker;
      lineMarker.header = poses[0].header;
      lineMarker.ns = ns + "_path";
      lineMarker.id = static_cast<int>(poses.size());
      lineMarker.type = visualization_msgs::msg::Marker::LINE_STRIP;
      lineMarker.action = visualization_msgs::msg::Marker::ADD;
 
      lineMarker.scale.x = markerScale_ * 0.3;  // Line width
 
      lineMarker.color.r = markerColor_[0];
      lineMarker.color.g = markerColor_[1];
      lineMarker.color.b = markerColor_[2];
      lineMarker.color.a = markerColor_[3] * 0.7;  // Slightly transparent
 
      for (const auto & pose : poses)
      {
        lineMarker.points.push_back(pose.pose.position);
      }
 
      markers_.markers.push_back(lineMarker);
    }
 
    markersReady_ = true;
    enabled_ = true;
 
    RCLCPP_INFO(
      getLogger(), "[CpTrajectoryVisualizer] Created %lu markers", markers_.markers.size());
  }

References enabled_, smacc2::ISmaccComponent::getLogger(), markerColor_, markers_, markerScale_, markersMutex_, and markersReady_.

Referenced by cl_moveit2z::CbMoveEndEffectorTrajectory::onEntry().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ update()

void cl_moveit2z::CpTrajectoryVisualizer::update ( )

inlineoverridevirtual

Periodic update - publishes markers if enabled.

Implements smacc2::ISmaccUpdatable.

Definition at line 72 of file cp_trajectory_visualizer.hpp.

  {
    std::lock_guard<std::mutex> guard(markersMutex_);
 
    if (!enabled_ || !markersReady_)
    {
      return;
    }
 
    // Rate limiting
    auto now = getNode()->now();
    auto elapsed = (now - lastPublishTime_).seconds();
    if (elapsed < (1.0 / publishRate_))
    {
      return;
    }
 
    markersPub_->publish(markers_);
    lastPublishTime_ = now;
  }