Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
platoon_strategic_ihp::Node Class Reference

ROS Node to for Platooning Strategic Plugin IHP2 variant. It includes all the service clients, publishers and subscribers. More...

#include <platoon_strategic_plugin_node_ihp.h>

Inheritance diagram for platoon_strategic_ihp::Node:
Inheritance graph
Collaboration diagram for platoon_strategic_ihp::Node:
Collaboration graph

Public Member Functions

 Node (const rclcpp::NodeOptions &)
 Node constructor. More...
 
rcl_interfaces::msg::SetParametersResult parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
 Callback for dynamic parameter updates. More...
 
void plan_maneuvers_callback (std::shared_ptr< rmw_request_id_t >, carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp) override
 Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
 
bool get_availability () override
 Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
 
std::string get_version_id () override
 Returns the version id of this plugin. More...
 
carma_ros2_utils::CallbackReturn on_configure_plugin ()
 This method should be used to load parameters and will be called on the configure state transition. More...
 
carma_ros2_utils::CallbackReturn on_cleanup_plugin ()
 Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...
 
- Public Member Functions inherited from carma_guidance_plugins::StrategicPlugin
 StrategicPlugin (const rclcpp::NodeOptions &)
 StrategicPlugin constructor. More...
 
virtual ~StrategicPlugin ()=default
 Virtual destructor for safe deletion. More...
 
virtual void plan_maneuvers_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp)=0
 Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...
 
std::string get_capability () override
 Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
 
uint8_t get_type () override final
 Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
 
carma_ros2_utils::CallbackReturn handle_on_configure (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_activate (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_deactivate (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_cleanup (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_shutdown (const rclcpp_lifecycle::State &) override final
 
carma_ros2_utils::CallbackReturn handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final
 
- Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
 PluginBaseNode (const rclcpp::NodeOptions &)
 PluginBaseNode constructor. More...
 
virtual ~PluginBaseNode ()=default
 Virtual destructor for safe deletion. More...
 
virtual std::shared_ptr< carma_wm::WMListenerget_world_model_listener () final
 Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
 
virtual carma_wm::WorldModelConstPtr get_world_model () final
 Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...
 
virtual bool get_activation_status () final
 Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...
 
virtual uint8_t get_type ()
 Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...
 
std::string get_plugin_name_and_ns () const
 Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...
 
std::string get_plugin_name () const
 Return the name of this plugin. More...
 
virtual bool get_availability ()=0
 Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...
 
virtual std::string get_capability ()=0
 Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...
 
virtual std::string get_version_id ()=0
 Returns the version id of this plugin. More...
 
virtual carma_ros2_utils::CallbackReturn on_configure_plugin ()=0
 Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...
 
virtual carma_ros2_utils::CallbackReturn on_activate_plugin ()
 Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...
 
virtual carma_ros2_utils::CallbackReturn on_deactivate_plugin ()
 Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...
 
virtual carma_ros2_utils::CallbackReturn on_cleanup_plugin ()
 Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...
 
virtual carma_ros2_utils::CallbackReturn on_shutdown_plugin ()
 Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...
 
virtual carma_ros2_utils::CallbackReturn on_error_plugin (const std::string &exception_string)
 Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...
 
carma_ros2_utils::CallbackReturn handle_on_configure (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_activate (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_deactivate (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_cleanup (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_shutdown (const rclcpp_lifecycle::State &) override
 
carma_ros2_utils::CallbackReturn handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override
 
 FRIEND_TEST (carma_guidance_plugins_test, connections_test)
 

Private Attributes

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::PlatooningInfo > platoon_info_pub
 
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityResponse > mob_response_pub
 
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityRequest > mob_request_pub
 
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_pub
 
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityRequest > mob_request_sub
 
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityResponse > mob_response_sub
 
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_sub
 
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > current_pose_sub
 
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > current_twist_sub
 
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > cmd_sub
 
carma_ros2_utils::SubPtr< std_msgs::msg::String > georeference_sub
 
rclcpp::TimerBase::SharedPtr loop_timer_
 
PlatoonPluginConfig config_
 
std::shared_ptr< PlatoonStrategicIHPPluginworker_
 

Detailed Description

ROS Node to for Platooning Strategic Plugin IHP2 variant. It includes all the service clients, publishers and subscribers.

Definition at line 51 of file platoon_strategic_plugin_node_ihp.h.

Constructor & Destructor Documentation

◆ Node()

platoon_strategic_ihp::Node::Node ( const rclcpp::NodeOptions &  options)
explicit

Node constructor.

Definition at line 24 of file platoon_strategic_ihp_node.cpp.

26 {
27 // Create initial config
28 config_ = PlatoonPluginConfig();
29
30 // Declare parameters
31
32 declare_parameter<bool>("allowCutinJoin", config_.allowCutinJoin);
33 declare_parameter<bool>("test_cutin_join", config_.test_cutin_join);
34 declare_parameter<bool>("test_front_join", config_.test_front_join);
35
36 declare_parameter<int>("maxPlatoonSize", config_.maxPlatoonSize);
37 declare_parameter<int>("algorithmType", config_.algorithmType);
38 declare_parameter<int>("statusMessageInterval", config_.statusMessageInterval);
39 declare_parameter<int>("infoMessageInterval", config_.infoMessageInterval);
40 declare_parameter<int>("join_index", config_.join_index);
41
42 declare_parameter<double>("timeHeadway", config_.timeHeadway);
43 declare_parameter<double>("standStillHeadway", config_.standStillHeadway);
44 declare_parameter<double>("maxAllowedJoinTimeGap", config_.maxAllowedJoinTimeGap);
45 declare_parameter<double>("maxAllowedJoinGap", config_.maxAllowedJoinGap);
46 declare_parameter<double>("minAllowedJoinGap", config_.minAllowedJoinGap);
47 declare_parameter<double>("desiredJoinTimeGap", config_.desiredJoinTimeGap);
48 declare_parameter<double>("desiredJoinGap", config_.desiredJoinGap);
49 declare_parameter<double>("waitingStateTimeout", config_.waitingStateTimeout);
50 declare_parameter<double>("cmdSpeedMaxAdjustment", config_.cmdSpeedMaxAdjustment);
51 declare_parameter<double>("minAllowableHeadaway", config_.minAllowableHeadaway);
52 declare_parameter<double>("maxAllowableHeadaway", config_.maxAllowableHeadaway);
53 declare_parameter<double>("headawayStableLowerBond", config_.headawayStableLowerBond);
54 declare_parameter<double>("headawayStableUpperBond", config_.headawayStableUpperBond);
55 declare_parameter<double>("minCutinGap", config_.minCutinGap);
56 declare_parameter<double>("maxCutinGap", config_.maxCutinGap);
57 declare_parameter<double>("maxCrosstrackError", config_.maxCrosstrackError);
58 declare_parameter<double>("minPlatooningSpeed", config_.minPlatooningSpeed);
59 declare_parameter<double>("significantDTDchange", config_.significantDTDchange);
60 declare_parameter<double>("longitudinalCheckThresold", config_.longitudinalCheckThresold);
61 declare_parameter<double>("vehicle_length", config_.vehicleLength);
62 declare_parameter<std::string>("vehicle_id", config_.vehicleID);
63 }
StrategicPlugin base class which can be extended by user provided plugins which wish to implement the...

References platoon_strategic_ihp::PlatoonPluginConfig::algorithmType, platoon_strategic_ihp::PlatoonPluginConfig::allowCutinJoin, platoon_strategic_ihp::PlatoonPluginConfig::cmdSpeedMaxAdjustment, config_, platoon_strategic_ihp::PlatoonPluginConfig::desiredJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::desiredJoinTimeGap, platoon_strategic_ihp::PlatoonPluginConfig::headawayStableLowerBond, platoon_strategic_ihp::PlatoonPluginConfig::headawayStableUpperBond, platoon_strategic_ihp::PlatoonPluginConfig::infoMessageInterval, platoon_strategic_ihp::PlatoonPluginConfig::join_index, platoon_strategic_ihp::PlatoonPluginConfig::longitudinalCheckThresold, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowableHeadaway, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowedJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowedJoinTimeGap, platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError, platoon_strategic_ihp::PlatoonPluginConfig::maxCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::maxPlatoonSize, platoon_strategic_ihp::PlatoonPluginConfig::minAllowableHeadaway, platoon_strategic_ihp::PlatoonPluginConfig::minAllowedJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::minCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::minPlatooningSpeed, platoon_strategic_ihp::PlatoonPluginConfig::significantDTDchange, platoon_strategic_ihp::PlatoonPluginConfig::standStillHeadway, platoon_strategic_ihp::PlatoonPluginConfig::statusMessageInterval, platoon_strategic_ihp::PlatoonPluginConfig::test_cutin_join, platoon_strategic_ihp::PlatoonPluginConfig::test_front_join, platoon_strategic_ihp::PlatoonPluginConfig::timeHeadway, platoon_strategic_ihp::PlatoonPluginConfig::vehicleID, platoon_strategic_ihp::PlatoonPluginConfig::vehicleLength, and platoon_strategic_ihp::PlatoonPluginConfig::waitingStateTimeout.

Member Function Documentation

◆ get_availability()

bool platoon_strategic_ihp::Node::get_availability ( )
overridevirtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns
This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 228 of file platoon_strategic_ihp_node.cpp.

228 {
229 return true;
230 }

◆ get_version_id()

std::string platoon_strategic_ihp::Node::get_version_id ( )
overridevirtual

Returns the version id of this plugin.

Returns
The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 232 of file platoon_strategic_ihp_node.cpp.

232 {
233 return "v4.0";
234 }

◆ on_cleanup_plugin()

carma_ros2_utils::CallbackReturn platoon_strategic_ihp::Node::on_cleanup_plugin ( )
virtual

Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched.

Returns
SUCCESS, FAILURE, or ERROR. Transition to UNCONFIGURED will only complete on SUCCESS.

Reimplemented from carma_guidance_plugins::PluginBaseNode.

Definition at line 204 of file platoon_strategic_ihp_node.cpp.

205 {
206 // Ensure subscribers are disconnected incase cleanup is called, we don't want to keep driving the worker
207 mob_response_sub.reset();
208 mob_operation_sub.reset();
209 current_pose_sub.reset();
210 current_twist_sub.reset();
211 cmd_sub.reset();
212 georeference_sub.reset();
213 worker_.reset();
214
215 return CallbackReturn::SUCCESS;
216 }
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_sub
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > cmd_sub
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > current_pose_sub
carma_ros2_utils::SubPtr< std_msgs::msg::String > georeference_sub
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > current_twist_sub
std::shared_ptr< PlatoonStrategicIHPPlugin > worker_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityResponse > mob_response_sub

References cmd_sub, current_pose_sub, current_twist_sub, georeference_sub, mob_operation_sub, mob_response_sub, and worker_.

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn platoon_strategic_ihp::Node::on_configure_plugin ( )
virtual

This method should be used to load parameters and will be called on the configure state transition.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 118 of file platoon_strategic_ihp_node.cpp.

119 {
120 // Reset config
121 config_ = PlatoonPluginConfig();
122
123 // Load parameters
124 get_parameter<bool>("test_front_join", config_.test_front_join);
125 get_parameter<bool>("allowCutinJoin", config_.allowCutinJoin);
126 get_parameter<bool>("test_cutin_join", config_.test_cutin_join);
127
128 get_parameter<int>("maxPlatoonSize", config_.maxPlatoonSize);
129 get_parameter<int>("algorithmType", config_.algorithmType);
130 get_parameter<int>("statusMessageInterval", config_.statusMessageInterval);
131 get_parameter<int>("infoMessageInterval", config_.infoMessageInterval);
132 get_parameter<int>("join_index", config_.join_index);
133
134 get_parameter<double>("timeHeadway", config_.timeHeadway);
135 get_parameter<double>("standStillHeadway", config_.standStillHeadway);
136 get_parameter<double>("maxAllowedJoinTimeGap", config_.maxAllowedJoinTimeGap);
137 get_parameter<double>("maxAllowedJoinGap", config_.maxAllowedJoinGap);
138 get_parameter<double>("minAllowedJoinGap", config_.minAllowedJoinGap);
139 get_parameter<double>("desiredJoinTimeGap", config_.desiredJoinTimeGap);
140 get_parameter<double>("desiredJoinGap", config_.desiredJoinGap);
141 get_parameter<double>("waitingStateTimeout", config_.waitingStateTimeout);
142 get_parameter<double>("cmdSpeedMaxAdjustment", config_.cmdSpeedMaxAdjustment);
143 get_parameter<double>("minAllowableHeadaway", config_.minAllowableHeadaway);
144 get_parameter<double>("maxAllowableHeadaway", config_.maxAllowableHeadaway);
145 get_parameter<double>("headawayStableLowerBond", config_.headawayStableLowerBond);
146 get_parameter<double>("headawayStableUpperBond", config_.headawayStableUpperBond);
147 get_parameter<double>("minCutinGap", config_.minCutinGap);
148 get_parameter<double>("maxCutinGap", config_.maxCutinGap);
149 get_parameter<double>("maxCrosstrackError", config_.maxCrosstrackError);
150 get_parameter<double>("minPlatooningSpeed", config_.minPlatooningSpeed);
151 get_parameter<double>("significantDTDchange", config_.significantDTDchange);
152 get_parameter<double>("longitudinalCheckThresold", config_.longitudinalCheckThresold);
153 get_parameter<double>("vehicle_length", config_.vehicleLength);
154 get_parameter<std::string>("vehicle_id", config_.vehicleID);
155
156 // Register runtime parameter update callback
157 add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1));
158
159 // Setup publishers
160 mob_response_pub = create_publisher<carma_v2x_msgs::msg::MobilityResponse>("outgoing_mobility_response", 5);
161 mob_request_pub = create_publisher<carma_v2x_msgs::msg::MobilityRequest>("outgoing_mobility_request", 5);
162 mob_operation_pub = create_publisher<carma_v2x_msgs::msg::MobilityOperation>("outgoing_mobility_operation", 5);
163 platoon_info_pub = create_publisher<carma_planning_msgs::msg::PlatooningInfo>("platoon_info", 1);
164
165 // Build worker
166
167 worker_ = std::make_shared<PlatoonStrategicIHPPlugin>(get_world_model(), config_, [this](auto msg) { this->mob_response_pub->publish(msg); },
168 [this](auto msg) { this->mob_request_pub->publish(msg); }, [this](auto msg) { this->mob_operation_pub->publish(msg); },
169 [this](auto msg) { this->platoon_info_pub->publish(msg); },
170 std::make_shared<carma_ros2_utils::timers::ROSTimerFactory>(shared_from_this()));
171
172
173 // Setup subscribers
174 mob_request_sub = create_subscription<carma_v2x_msgs::msg::MobilityRequest>("incoming_mobility_request", 10,
175 std::bind(&PlatoonStrategicIHPPlugin::mob_req_cb, worker_.get(), std_ph::_1));
176
177 mob_response_sub = create_subscription<carma_v2x_msgs::msg::MobilityResponse>("incoming_mobility_response", 10,
178 std::bind(&PlatoonStrategicIHPPlugin::mob_resp_cb, worker_.get(), std_ph::_1));
179
180 mob_operation_sub = create_subscription<carma_v2x_msgs::msg::MobilityOperation>("incoming_mobility_operation", 10,
181 std::bind(&PlatoonStrategicIHPPlugin::mob_op_cb, worker_.get(), std_ph::_1));
182
183 current_pose_sub = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 10,
184 std::bind(&PlatoonStrategicIHPPlugin::pose_cb, worker_.get(), std_ph::_1));
185
186 current_twist_sub = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 10,
187 std::bind(&PlatoonStrategicIHPPlugin::twist_cb, worker_.get(), std_ph::_1));
188
189 cmd_sub = create_subscription<geometry_msgs::msg::TwistStamped>("twist_raw", 10,
190 std::bind(&PlatoonStrategicIHPPlugin::cmd_cb, worker_.get(), std_ph::_1));
191
192 georeference_sub = create_subscription<std_msgs::msg::String>("georeference", 10,
193 std::bind(&PlatoonStrategicIHPPlugin::georeference_cb, worker_.get(), std_ph::_1));
194
195 loop_timer_ = create_timer(
196 get_clock(),
197 std::chrono::milliseconds(100), // 10 Hz frequency
198 std::bind(&PlatoonStrategicIHPPlugin::onSpin, worker_.get()));
199
200 // Return success if everything initialized successfully
201 return CallbackReturn::SUCCESS;
202 }
virtual carma_wm::WorldModelConstPtr get_world_model() final
Method to return the default world model provided as a convience by this base class If this method or...
rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector< rclcpp::Parameter > &parameters)
Callback for dynamic parameter updates.
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityRequest > mob_request_sub
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityResponse > mob_response_pub
rclcpp::TimerBase::SharedPtr loop_timer_
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityOperation > mob_operation_pub
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityRequest > mob_request_pub
carma_ros2_utils::PubPtr< carma_planning_msgs::msg::PlatooningInfo > platoon_info_pub
void georeference_cb(const std_msgs::msg::String::UniquePtr msg)
Callback for the georeference.
void mob_req_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg)
Callback function for Mobility Request Message.
void mob_op_cb(const carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
Callback function for Mobility Operation Message.
void cmd_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
Callback for the control command.
void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg)
Callback function for current pose.
void mob_resp_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg)
Callback function for Mobility Response Message.
void twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
Callback for the twist subscriber, which will store latest twist locally.

References platoon_strategic_ihp::PlatoonPluginConfig::algorithmType, platoon_strategic_ihp::PlatoonPluginConfig::allowCutinJoin, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::cmd_cb(), cmd_sub, platoon_strategic_ihp::PlatoonPluginConfig::cmdSpeedMaxAdjustment, config_, current_pose_sub, current_twist_sub, platoon_strategic_ihp::PlatoonPluginConfig::desiredJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::desiredJoinTimeGap, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::georeference_cb(), georeference_sub, carma_guidance_plugins::PluginBaseNode::get_world_model(), platoon_strategic_ihp::PlatoonPluginConfig::headawayStableLowerBond, platoon_strategic_ihp::PlatoonPluginConfig::headawayStableUpperBond, platoon_strategic_ihp::PlatoonPluginConfig::infoMessageInterval, platoon_strategic_ihp::PlatoonPluginConfig::join_index, platoon_strategic_ihp::PlatoonPluginConfig::longitudinalCheckThresold, loop_timer_, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowableHeadaway, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowedJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowedJoinTimeGap, platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError, platoon_strategic_ihp::PlatoonPluginConfig::maxCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::maxPlatoonSize, platoon_strategic_ihp::PlatoonPluginConfig::minAllowableHeadaway, platoon_strategic_ihp::PlatoonPluginConfig::minAllowedJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::minCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::minPlatooningSpeed, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_op_cb(), mob_operation_pub, mob_operation_sub, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_req_cb(), mob_request_pub, mob_request_sub, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::mob_resp_cb(), mob_response_pub, mob_response_sub, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::onSpin(), parameter_update_callback(), platoon_info_pub, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::pose_cb(), platoon_strategic_ihp::PlatoonPluginConfig::significantDTDchange, platoon_strategic_ihp::PlatoonPluginConfig::standStillHeadway, platoon_strategic_ihp::PlatoonPluginConfig::statusMessageInterval, platoon_strategic_ihp::PlatoonPluginConfig::test_cutin_join, platoon_strategic_ihp::PlatoonPluginConfig::test_front_join, platoon_strategic_ihp::PlatoonPluginConfig::timeHeadway, platoon_strategic_ihp::PlatoonStrategicIHPPlugin::twist_cb(), platoon_strategic_ihp::PlatoonPluginConfig::vehicleID, platoon_strategic_ihp::PlatoonPluginConfig::vehicleLength, platoon_strategic_ihp::PlatoonPluginConfig::waitingStateTimeout, and worker_.

Here is the call graph for this function:

◆ parameter_update_callback()

rcl_interfaces::msg::SetParametersResult platoon_strategic_ihp::Node::parameter_update_callback ( const std::vector< rclcpp::Parameter > &  parameters)

Callback for dynamic parameter updates.

Definition at line 65 of file platoon_strategic_ihp_node.cpp.

66 {
67
68 auto error = update_params<bool>({
69 {"test_front_join", config_.test_front_join},
70 {"allowCutinJoin", config_.allowCutinJoin},
71 {"test_cutin_join", config_.test_cutin_join}
72 }, parameters);
73
74 auto error2 = update_params<int>({
75 {"maxPlatoonSize", config_.maxPlatoonSize},
76 {"algorithmType", config_.algorithmType},
77 {"statusMessageInterval", config_.statusMessageInterval},
78 {"infoMessageInterval", config_.infoMessageInterval},
79 {"join_index", config_.join_index}
80 }, parameters);
81
82 auto error3 = update_params<double>({
83 {"timeHeadway", config_.timeHeadway},
84 {"standStillHeadway", config_.standStillHeadway},
85 {"maxAllowedJoinTimeGap", config_.maxAllowedJoinTimeGap},
86 {"maxAllowedJoinGap", config_.maxAllowedJoinGap},
87 {"minAllowedJoinGap", config_.minAllowedJoinGap},
88 {"desiredJoinTimeGap", config_.desiredJoinTimeGap},
89 {"desiredJoinGap", config_.desiredJoinGap},
90 {"waitingStateTimeout", config_.waitingStateTimeout},
91 {"cmdSpeedMaxAdjustment", config_.cmdSpeedMaxAdjustment},
92 {"minAllowableHeadaway", config_.minAllowableHeadaway},
93 {"maxAllowableHeadaway", config_.maxAllowableHeadaway},
94 {"headawayStableLowerBond", config_.headawayStableLowerBond},
95 {"headawayStableUpperBond", config_.headawayStableUpperBond},
96 {"minCutinGap", config_.minCutinGap},
97 {"maxCutinGap", config_.maxCutinGap},
98 {"maxCrosstrackError", config_.maxCrosstrackError},
99 {"minPlatooningSpeed", config_.minPlatooningSpeed},
100 {"significantDTDchange", config_.significantDTDchange},
101 {"longitudinalCheckThresold", config_.longitudinalCheckThresold}
102 // vehicle_length and id excluded since they should not be updated at runtime
103 }, parameters);
104
105
106 rcl_interfaces::msg::SetParametersResult result;
107
108 result.successful = !error && !error2 && !error3;
109
110 if (result.successful && worker_)
111 {
112 worker_->setConfig(config_);
113 }
114
115 return result;
116 }

References platoon_strategic_ihp::PlatoonPluginConfig::algorithmType, platoon_strategic_ihp::PlatoonPluginConfig::allowCutinJoin, platoon_strategic_ihp::PlatoonPluginConfig::cmdSpeedMaxAdjustment, config_, platoon_strategic_ihp::PlatoonPluginConfig::desiredJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::desiredJoinTimeGap, platoon_strategic_ihp::PlatoonPluginConfig::headawayStableLowerBond, platoon_strategic_ihp::PlatoonPluginConfig::headawayStableUpperBond, platoon_strategic_ihp::PlatoonPluginConfig::infoMessageInterval, platoon_strategic_ihp::PlatoonPluginConfig::join_index, platoon_strategic_ihp::PlatoonPluginConfig::longitudinalCheckThresold, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowableHeadaway, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowedJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::maxAllowedJoinTimeGap, platoon_strategic_ihp::PlatoonPluginConfig::maxCrosstrackError, platoon_strategic_ihp::PlatoonPluginConfig::maxCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::maxPlatoonSize, platoon_strategic_ihp::PlatoonPluginConfig::minAllowableHeadaway, platoon_strategic_ihp::PlatoonPluginConfig::minAllowedJoinGap, platoon_strategic_ihp::PlatoonPluginConfig::minCutinGap, platoon_strategic_ihp::PlatoonPluginConfig::minPlatooningSpeed, platoon_strategic_ihp::PlatoonPluginConfig::significantDTDchange, platoon_strategic_ihp::PlatoonPluginConfig::standStillHeadway, platoon_strategic_ihp::PlatoonPluginConfig::statusMessageInterval, platoon_strategic_ihp::PlatoonPluginConfig::test_cutin_join, platoon_strategic_ihp::PlatoonPluginConfig::test_front_join, platoon_strategic_ihp::PlatoonPluginConfig::timeHeadway, platoon_strategic_ihp::PlatoonPluginConfig::waitingStateTimeout, and worker_.

Referenced by on_configure_plugin().

Here is the caller graph for this function:

◆ plan_maneuvers_callback()

void platoon_strategic_ihp::Node::plan_maneuvers_callback ( std::shared_ptr< rmw_request_id_t >  srv_header,
carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr  req,
carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr  resp 
)
overridevirtual

Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request.

Parameters
srv_headerRCL header for services calls. Can usually be ignored by implementers.
reqThe service request containing the previously planned maneuvers and vehicle state
respThe response containing the additional maneuvers generated by this plugin

Implements carma_guidance_plugins::StrategicPlugin.

Definition at line 219 of file platoon_strategic_ihp_node.cpp.

223 {
224 if (worker_)
225 worker_->plan_maneuver_cb(*req, *resp);
226 }

References worker_.

Member Data Documentation

◆ cmd_sub

carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> platoon_strategic_ihp::Node::cmd_sub
private

Definition at line 68 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_cleanup_plugin(), and on_configure_plugin().

◆ config_

PlatoonPluginConfig platoon_strategic_ihp::Node::config_
private

◆ current_pose_sub

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> platoon_strategic_ihp::Node::current_pose_sub
private

Definition at line 66 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_cleanup_plugin(), and on_configure_plugin().

◆ current_twist_sub

carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> platoon_strategic_ihp::Node::current_twist_sub
private

Definition at line 67 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_cleanup_plugin(), and on_configure_plugin().

◆ georeference_sub

carma_ros2_utils::SubPtr<std_msgs::msg::String> platoon_strategic_ihp::Node::georeference_sub
private

Definition at line 69 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_cleanup_plugin(), and on_configure_plugin().

◆ loop_timer_

rclcpp::TimerBase::SharedPtr platoon_strategic_ihp::Node::loop_timer_
private

Definition at line 72 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_configure_plugin().

◆ mob_operation_pub

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityOperation> platoon_strategic_ihp::Node::mob_operation_pub
private

Definition at line 60 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_configure_plugin().

◆ mob_operation_sub

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityOperation> platoon_strategic_ihp::Node::mob_operation_sub
private

Definition at line 65 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_cleanup_plugin(), and on_configure_plugin().

◆ mob_request_pub

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityRequest> platoon_strategic_ihp::Node::mob_request_pub
private

Definition at line 59 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_configure_plugin().

◆ mob_request_sub

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityRequest> platoon_strategic_ihp::Node::mob_request_sub
private

Definition at line 63 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_configure_plugin().

◆ mob_response_pub

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityResponse> platoon_strategic_ihp::Node::mob_response_pub
private

Definition at line 58 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_configure_plugin().

◆ mob_response_sub

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityResponse> platoon_strategic_ihp::Node::mob_response_sub
private

Definition at line 64 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_cleanup_plugin(), and on_configure_plugin().

◆ platoon_info_pub

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::PlatooningInfo> platoon_strategic_ihp::Node::platoon_info_pub
private

Definition at line 57 of file platoon_strategic_plugin_node_ihp.h.

Referenced by on_configure_plugin().

◆ worker_

std::shared_ptr<PlatoonStrategicIHPPlugin> platoon_strategic_ihp::Node::worker_
private

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