SMACC2
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | Protected Member Functions | Private Member Functions | Private Attributes | List of all members
cl_nav2z::CpWaypointsVisualizer Class Reference

#include <waypoints_visualizer.hpp>

Inheritance diagram for cl_nav2z::CpWaypointsVisualizer:
Inheritance graph
Collaboration diagram for cl_nav2z::CpWaypointsVisualizer:
Collaboration graph

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::WaypointNavigatorwaypointsNavigator_
 

Protected Member Functions

virtual void update () override
 
- Protected Member Functions inherited from smacc2::ISmaccComponent
virtual void onInitialize ()
 
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
 
ISmaccStateMachinegetStateMachine ()
 
virtual void update ()=0
 

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
ISmaccStateMachinestateMachine_
 
ISmaccClientowner_
 

Detailed Description

Definition at line 34 of file waypoints_visualizer.hpp.

Constructor & Destructor Documentation

◆ CpWaypointsVisualizer()

cl_nav2z::CpWaypointsVisualizer::CpWaypointsVisualizer ( rclcpp::Duration  duration)

Definition at line 27 of file waypoints_visualizer.cpp.

27 : ISmaccUpdatable(duration)
28{
29}

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 waypoints_visualizer.cpp.

88{
89 marker.header.frame_id = frameid;
90 marker.header.stamp = getNode()->now();
91 marker.ns = "waypoints";
92
93 marker.id = markers_.markers.size();
94 marker.type = visualization_msgs::msg::Marker::SPHERE;
95 marker.action = visualization_msgs::msg::Marker::ADD;
96 marker.scale.x = 0.1;
97 marker.scale.y = 0.1;
98 marker.scale.z = 0.1;
99
100 marker.color.a = 1.0;
101 marker.pose = waypoint;
102}
visualization_msgs::msg::MarkerArray markers_
rclcpp::Node::SharedPtr getNode()
const std::string frameid

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 waypoints_visualizer.cpp.

68{
69 marker.header.frame_id = frameid;
70 marker.header.stamp = getNode()->now();
71 marker.ns = "waypoints_labels";
72
73 marker.id = markers_.markers.size();
74 marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
75 marker.action = visualization_msgs::msg::Marker::ADD;
76 marker.scale.x = 0.3;
77 marker.scale.y = 0.3;
78 marker.scale.z = 0.3;
79 marker.text = label;
80
81 marker.color.a = 1.0;
82 marker.pose = waypoint;
83 marker.pose.position.z += 0.3;
84}

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 waypoints_visualizer.cpp.

32{
33 markersPub_ = getNode()->create_publisher<visualization_msgs::msg::MarkerArray>(
34 "cp_waypoints_visualizer/visualization_markers", rclcpp::QoS(rclcpp::KeepLast(1)));
35
37 auto & waypoints = waypointsNavigator_->getWaypoints();
38 auto & waypointsNames = waypointsNavigator_->getWaypointNames();
39
40 int i = 0;
41 for (auto & waypoint : waypoints)
42 {
43 std::string name;
44 if ((long)waypointsNames.size() > i)
45 {
46 name = waypointsNames[i];
47 }
48 else
49 {
50 name = "waypoint_" + std::to_string(i);
51 }
52
53 visualization_msgs::msg::Marker marker;
54 createMarker(waypoint, marker);
55 markers_.markers.push_back(marker);
56
57 visualization_msgs::msg::Marker markerlabel;
58 createMarkerLabel(waypoint, name, markerlabel);
59 markerLabels_.markers.push_back(markerlabel);
60
61 i++;
62 }
63}
visualization_msgs::msg::MarkerArray markerLabels_
rclcpp::Publisher< visualization_msgs::msg::MarkerArray >::SharedPtr markersPub_
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)
cl_nav2z::WaypointNavigator * waypointsNavigator_
const std::vector< geometry_msgs::msg::Pose > & getWaypoints() const
const std::vector< std::string > & getWaypointNames() const
void requiresComponent(TComponent *&requiredComponentStorage, bool throwExceptionIfNotExist=false)

References createMarker(), createMarkerLabel(), smacc2::ISmaccComponent::getNode(), cl_nav2z::WaypointNavigator::getWaypointNames(), cl_nav2z::WaypointNavigator::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 waypoints_visualizer.cpp.

105{
106 std::lock_guard<std::mutex> guard(m_mutex_);
107
109
110 int i = 0;
111
112 for (auto & marker : markers_.markers)
113 {
114 marker.header.stamp = getNode()->now();
115
116 if (i >= index)
117 {
118 marker.color.r = 1.0;
119 marker.color.g = 0;
120 marker.color.b = 0;
121 }
122 else
123 {
124 marker.color.r = 0.0;
125 marker.color.g = 1.0;
126 marker.color.b = 0;
127 }
128 i++;
129 }
130
131 i = 0;
132 for (auto & marker : markerLabels_.markers)
133 {
134 marker.header.stamp = getNode()->now();
135
136 if (i >= index)
137 {
138 marker.color.r = 1.0;
139 marker.color.g = 0;
140 marker.color.b = 0;
141 }
142 else
143 {
144 marker.color.r = 0.0;
145 marker.color.g = 1.0;
146 marker.color.b = 0;
147 }
148 i++;
149 }
150
151 //markers_.header.stamp = getNode()->now();
152 markersPub_->publish(markers_);
153 markersPub_->publish(markerLabels_);
154}

References cl_nav2z::WaypointNavigator::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 waypoints_visualizer.hpp.

Referenced by update().

◆ markerLabels_

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

Definition at line 51 of file waypoints_visualizer.hpp.

Referenced by onInitialize(), and update().

◆ markers_

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

Definition at line 50 of file 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 waypoints_visualizer.hpp.

Referenced by onInitialize(), and update().

◆ waypointsNavigator_

cl_nav2z::WaypointNavigator* cl_nav2z::CpWaypointsVisualizer::waypointsNavigator_

Definition at line 37 of file waypoints_visualizer.hpp.

Referenced by onInitialize(), and update().


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