cl_nav2z::CpWaypointsVisualizer Class Reference

#include <cp_waypoints_visualizer.hpp>

Inheritance diagram for cl_nav2z::CpWaypointsVisualizer:

Collaboration diagram for cl_nav2z::CpWaypointsVisualizer:

Public Member Functions
	CpWaypointsVisualizer (rclcpp::Duration duration)
void	onInitialize () override
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)

Public Attributes
cl_nav2z::CpWaypointNavigator *	waypointsNavigator_

Protected Member Functions
virtual void	update () override
Protected Member Functions inherited from smacc2::ISmaccComponent
template<typename EventType >
void	postEvent (const EventType &ev)
template<typename EventType >
void	postEvent ()
template<typename TOrthogonal , typename TSourceObject >
void	onOrthogonalAllocation ()
template<typename TComponent >
void	requiresComponent (TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)
template<typename TComponent >
void	requiresComponent (std::string name, TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)
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 ()

Private Member Functions
void	createMarker (const geometry_msgs::msg::Pose &waypoint, visualization_msgs::msg::Marker &m)
void	createMarkerLabel (const geometry_msgs::msg::Pose &waypoint, std::string label, visualization_msgs::msg::Marker &m)

Private Attributes
std::mutex	m_mutex_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr	markersPub_
visualization_msgs::msg::MarkerArray	markers_
visualization_msgs::msg::MarkerArray	markerLabels_

Additional Inherited Members
Protected Attributes inherited from smacc2::ISmaccComponent
ISmaccStateMachine *	stateMachine_
ISmaccClient *	owner_

Detailed Description

Definition at line 34 of file cp_waypoints_visualizer.hpp.

Constructor & Destructor Documentation

CpWaypointsVisualizer()

cl_nav2z::CpWaypointsVisualizer::CpWaypointsVisualizer

(

rclcpp::Duration

duration

)

Definition at line 27 of file cp_waypoints_visualizer.cpp.

                                                                    : ISmaccUpdatable(duration)
{
}

Member Function Documentation

createMarker()

void cl_nav2z::CpWaypointsVisualizer::createMarker ( const geometry_msgs::msg::Pose &  waypoint, visualization_msgs::msg::Marker &  m  )

private

Definition at line 86 of file cp_waypoints_visualizer.cpp.

{
  marker.header.frame_id = frameid;
  marker.header.stamp = getNode()->now();
  marker.ns = "waypoints";
 
  marker.id = markers_.markers.size();
  marker.type = visualization_msgs::msg::Marker::SPHERE;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.scale.x = 0.1;
  marker.scale.y = 0.1;
  marker.scale.z = 0.1;
 
  marker.color.a = 1.0;
  marker.pose = waypoint;
}

References cl_nav2z::frameid, smacc2::ISmaccComponent::getNode(), and markers_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

createMarkerLabel()

void cl_nav2z::CpWaypointsVisualizer::createMarkerLabel ( const geometry_msgs::msg::Pose &  waypoint, std::string  label, visualization_msgs::msg::Marker &  m  )

private

Definition at line 65 of file cp_waypoints_visualizer.cpp.

{
  marker.header.frame_id = frameid;
  marker.header.stamp = getNode()->now();
  marker.ns = "waypoints_labels";
 
  marker.id = markers_.markers.size();
  marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
  marker.action = visualization_msgs::msg::Marker::ADD;
  marker.scale.x = 0.3;
  marker.scale.y = 0.3;
  marker.scale.z = 0.3;
  marker.text = label;
 
  marker.color.a = 1.0;
  marker.pose = waypoint;
  marker.pose.position.z += 0.3;
}

References cl_nav2z::frameid, smacc2::ISmaccComponent::getNode(), and markers_.

Referenced by onInitialize().

Here is the call graph for this function:

Here is the caller graph for this function:

onInitialize()

void cl_nav2z::CpWaypointsVisualizer::onInitialize ( )

overridevirtual

Reimplemented from smacc2::ISmaccComponent.

Definition at line 31 of file cp_waypoints_visualizer.cpp.

{
  markersPub_ = getNode()->create_publisher<visualization_msgs::msg::MarkerArray>(
    "cp_waypoints_visualizer/visualization_markers", rclcpp::QoS(rclcpp::KeepLast(1)));
 
  this->requiresComponent(waypointsNavigator_);
  auto & waypoints = waypointsNavigator_->getWaypoints();
  auto & waypointsNames = waypointsNavigator_->getWaypointNames();
 
  int i = 0;
  for (auto & waypoint : waypoints)
  {
    std::string name;
    if ((long)waypointsNames.size() > i)
    {
      name = waypointsNames[i];
    }
    else
    {
      name = "waypoint_" + std::to_string(i);
    }
 
    visualization_msgs::msg::Marker marker;
    createMarker(waypoint, marker);
    markers_.markers.push_back(marker);
 
    visualization_msgs::msg::Marker markerlabel;
    createMarkerLabel(waypoint, name, markerlabel);
    markerLabels_.markers.push_back(markerlabel);
 
    i++;
  }
}

References createMarker(), createMarkerLabel(), smacc2::ISmaccComponent::getNode(), cl_nav2z::CpWaypointNavigatorBase::getWaypointNames(), cl_nav2z::CpWaypointNavigatorBase::getWaypoints(), markerLabels_, markers_, markersPub_, smacc2::ISmaccComponent::requiresComponent(), and waypointsNavigator_.

Here is the call graph for this function:

update()

void cl_nav2z::CpWaypointsVisualizer::update ( )

overrideprotectedvirtual

Implements smacc2::ISmaccUpdatable.

Definition at line 104 of file cp_waypoints_visualizer.cpp.

{
  std::lock_guard<std::mutex> guard(m_mutex_);
 
  auto index = waypointsNavigator_->getCurrentWaypointIndex();
 
  int i = 0;
 
  for (auto & marker : markers_.markers)
  {
    marker.header.stamp = getNode()->now();
 
    if (i >= index)
    {
      marker.color.r = 1.0;
      marker.color.g = 0;
      marker.color.b = 0;
    }
    else
    {
      marker.color.r = 0.0;
      marker.color.g = 1.0;
      marker.color.b = 0;
    }
    i++;
  }
 
  i = 0;
  for (auto & marker : markerLabels_.markers)
  {
    marker.header.stamp = getNode()->now();
 
    if (i >= index)
    {
      marker.color.r = 1.0;
      marker.color.g = 0;
      marker.color.b = 0;
    }
    else
    {
      marker.color.r = 0.0;
      marker.color.g = 1.0;
      marker.color.b = 0;
    }
    i++;
  }
 
  //markers_.header.stamp = getNode()->now();
  markersPub_->publish(markers_);
  markersPub_->publish(markerLabels_);
}

References cl_nav2z::CpWaypointNavigatorBase::getCurrentWaypointIndex(), smacc2::ISmaccComponent::getNode(), m_mutex_, markerLabels_, markers_, markersPub_, and waypointsNavigator_.

Here is the call graph for this function:

Member Data Documentation

m_mutex_

std::mutex cl_nav2z::CpWaypointsVisualizer::m_mutex_

private

Definition at line 47 of file cp_waypoints_visualizer.hpp.

Referenced by update().

markerLabels_

visualization_msgs::msg::MarkerArray cl_nav2z::CpWaypointsVisualizer::markerLabels_

private

Definition at line 51 of file cp_waypoints_visualizer.hpp.

Referenced by onInitialize(), and update().

markers_

visualization_msgs::msg::MarkerArray cl_nav2z::CpWaypointsVisualizer::markers_

private

Definition at line 50 of file cp_waypoints_visualizer.hpp.

Referenced by createMarker(), createMarkerLabel(), onInitialize(), and update().

markersPub_

rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr cl_nav2z::CpWaypointsVisualizer::markersPub_

private

Definition at line 49 of file cp_waypoints_visualizer.hpp.

Referenced by onInitialize(), and update().

waypointsNavigator_

cl_nav2z::CpWaypointNavigator* cl_nav2z::CpWaypointsVisualizer::waypointsNavigator_

Definition at line 37 of file cp_waypoints_visualizer.hpp.

Referenced by onInitialize(), and update().

---

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

• smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_visualizer.hpp
• smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/cp_waypoints_visualizer.cpp