guidance::GuidanceWorker Class Reference

Worker class for the Guidance node. More...

#include <guidance_worker.hpp>

Inheritance diagram for guidance::GuidanceWorker:

Collaboration diagram for guidance::GuidanceWorker:

Public Member Functions
	GuidanceWorker (const rclcpp::NodeOptions &)
	GuidanceWorker constructor. More...
rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Callback for dynamic parameter updates. More...
bool	spin_cb ()
	Timer callback. More...
void	route_event_cb (carma_planning_msgs::msg::RouteEvent::UniquePtr msg)
	Callback for route event messages. More...
void	robot_status_cb (carma_driver_msgs::msg::RobotEnabled::UniquePtr msg)
	Callback for robot enabled messages. More...
void	vehicle_status_cb (const autoware_msgs::msg::VehicleStatus::UniquePtr msg)
	Callback for vehicle status messages. More...
bool	guidance_activation_cb (const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< carma_planning_msgs::srv::SetGuidanceActive::Request > req, std::shared_ptr< carma_planning_msgs::srv::SetGuidanceActive::Response > resp)
	Set_guidance_active service callback. User can attempt to activate the guidance system by calling this service. More...
carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &prev_state)
carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)
carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &prev_state)

Private Attributes
carma_ros2_utils::SubPtr< carma_driver_msgs::msg::RobotEnabled >	robot_status_subscriber_
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::RouteEvent >	route_event_subscriber_
carma_ros2_utils::SubPtr< autoware_msgs::msg::VehicleStatus >	vehicle_status_subscriber_
carma_ros2_utils::PubPtr< carma_planning_msgs::msg::GuidanceState >	state_publisher_
carma_ros2_utils::ClientPtr< carma_driver_msgs::srv::SetEnableRobotic >	enable_client_
carma_ros2_utils::ServicePtr< carma_planning_msgs::srv::SetGuidanceActive >	guidance_activate_service_server_
rclcpp::TimerBase::SharedPtr	spin_timer_
Config	config_
GuidanceStateMachine	gsm_

Detailed Description

Worker class for the Guidance node.

Definition at line 39 of file guidance_worker.hpp.

Constructor & Destructor Documentation

GuidanceWorker()

guidance::GuidanceWorker::GuidanceWorker ( const rclcpp::NodeOptions &  options )

explicit

GuidanceWorker constructor.

Definition at line 22 of file guidance_worker.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options),
        gsm_(this->get_node_logging_interface())
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.spin_rate_hz = declare_parameter<double>("spin_rate_hz", config_.spin_rate_hz);
  }

References config_, and guidance::Config::spin_rate_hz.

Member Function Documentation

guidance_activation_cb()

bool guidance::GuidanceWorker::guidance_activation_cb	(	const std::shared_ptr< rmw_request_id_t >	,
		const std::shared_ptr< carma_planning_msgs::srv::SetGuidanceActive::Request >	req,
		std::shared_ptr< carma_planning_msgs::srv::SetGuidanceActive::Response >	resp
	)

Set_guidance_active service callback. User can attempt to activate the guidance system by calling this service.

Parameters

req	A carma_planning_msgs::srv::SetGuidanceActive::Request msg
resp	A carma_planning_msgs::srv::SetGuidanceActive::Response msg

Definition at line 102 of file guidance_worker.cpp.

  {
    // Translate message type from GuidanceActiveRequest to SetEnableRobotic
    if(!req->guidance_active)
    {
      auto enable_robotic_request = std::make_shared<carma_driver_msgs::srv::SetEnableRobotic::Request>();
      enable_robotic_request->set = carma_driver_msgs::srv::SetEnableRobotic::Request::DISABLE;
      enable_client_->async_send_request(enable_robotic_request);
    }
 
    gsm_.onSetGuidanceActive(req->guidance_active);
    resp->guidance_status = (gsm_.getCurrentState() == GuidanceStateMachine::ACTIVE);
    return true;  
  }

References guidance::GuidanceStateMachine::ACTIVE, enable_client_, guidance::GuidanceStateMachine::getCurrentState(), gsm_, and guidance::GuidanceStateMachine::onSetGuidanceActive().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

handle_on_activate()

carma_ros2_utils::CallbackReturn guidance::GuidanceWorker::handle_on_activate

(

const rclcpp_lifecycle::State &

prev_state

)

Definition at line 81 of file guidance_worker.cpp.

  {
    // Setup timer
    auto guidance_spin_period_ms = int ((1 / config_.spin_rate_hz) * 1000); // Conversion rom frequency (Hz) to milliseconds time period
    spin_timer_ = create_timer(
        get_clock(),
        std::chrono::milliseconds(guidance_spin_period_ms),
        std::bind(&GuidanceWorker::spin_cb, this));
 
    // Update Guidance State Machine since node activation by the lifecycle system indicates that all required drivers are ready
    gsm_.onGuidanceInitialized();
 
    return CallbackReturn::SUCCESS;
  }

References config_, gsm_, guidance::GuidanceStateMachine::onGuidanceInitialized(), spin_cb(), guidance::Config::spin_rate_hz, and spin_timer_.

Here is the call graph for this function:

handle_on_configure()

carma_ros2_utils::CallbackReturn guidance::GuidanceWorker::handle_on_configure

(

const rclcpp_lifecycle::State &

prev_state

)

Definition at line 44 of file guidance_worker.cpp.

  {
    RCLCPP_INFO_STREAM(get_logger(), "GuidanceWorker trying to configure");
 
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("spin_rate_hz", config_.spin_rate_hz);
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&GuidanceWorker::parameter_update_callback, this, std_ph::_1));
 
    // Setup subscribers
    robot_status_subscriber_ = create_subscription<carma_driver_msgs::msg::RobotEnabled>("robot_status", 5,
                                                              std::bind(&GuidanceWorker::robot_status_cb, this, std_ph::_1));
    route_event_subscriber_ = create_subscription<carma_planning_msgs::msg::RouteEvent>("route_event", 5,
                                                              std::bind(&GuidanceWorker::route_event_cb, this, std_ph::_1));
    vehicle_status_subscriber_ = create_subscription<autoware_msgs::msg::VehicleStatus>("vehicle_status", 5,
                                                              std::bind(&GuidanceWorker::vehicle_status_cb, this, std_ph::_1));
 
    // Setup publishers
    state_publisher_  = create_publisher<carma_planning_msgs::msg::GuidanceState>("state", 5);
 
    // Setup service clients
    enable_client_  = create_client<carma_driver_msgs::srv::SetEnableRobotic>("enable_robotic");
 
    // Setup service servers
    guidance_activate_service_server_  = create_service<carma_planning_msgs::srv::SetGuidanceActive>("set_guidance_active",
                                                            std::bind(&GuidanceWorker::guidance_activation_cb, this, std_ph::_1, std_ph::_2, std_ph::_3));
 
    // Return success if everything initialized successfully
    return CallbackReturn::SUCCESS;
  }

References config_, enable_client_, guidance_activate_service_server_, guidance_activation_cb(), parameter_update_callback(), robot_status_cb(), robot_status_subscriber_, route_event_cb(), route_event_subscriber_, guidance::Config::spin_rate_hz, state_publisher_, vehicle_status_cb(), and vehicle_status_subscriber_.

Here is the call graph for this function:

handle_on_shutdown()

carma_ros2_utils::CallbackReturn guidance::GuidanceWorker::handle_on_shutdown

(

const rclcpp_lifecycle::State &

prev_state

)

Definition at line 96 of file guidance_worker.cpp.

  {
    // Update Guidance State Machine with SHUTDOWN signal
    gsm_.onGuidanceShutdown();
  }

References gsm_, and guidance::GuidanceStateMachine::onGuidanceShutdown().

Here is the call graph for this function:

parameter_update_callback()

rcl_interfaces::msg::SetParametersResult guidance::GuidanceWorker::parameter_update_callback

(

const std::vector< rclcpp::Parameter > &

parameters

)

Callback for dynamic parameter updates.

Definition at line 33 of file guidance_worker.cpp.

  {
    auto error = update_params<double>({{"spin_rate_hz", config_.spin_rate_hz}}, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error;
 
    return result;
  }

References config_, and guidance::Config::spin_rate_hz.

Referenced by handle_on_configure().

Here is the caller graph for this function:

robot_status_cb()

void guidance::GuidanceWorker::robot_status_cb

(

carma_driver_msgs::msg::RobotEnabled::UniquePtr

msg

)

Callback for robot enabled messages.

Parameters

msg	Latest robot enabled message

Definition at line 138 of file guidance_worker.cpp.

  {
    gsm_.onRoboticStatus(move(msg));
  }

References gsm_, and guidance::GuidanceStateMachine::onRoboticStatus().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

route_event_cb()

void guidance::GuidanceWorker::route_event_cb

(

carma_planning_msgs::msg::RouteEvent::UniquePtr

msg

)

Callback for route event messages.

Parameters

msg	Latest route event message

Definition at line 133 of file guidance_worker.cpp.

  {
    gsm_.onRouteEvent(move(msg));
  }

References gsm_, and guidance::GuidanceStateMachine::onRouteEvent().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

spin_cb()

bool guidance::GuidanceWorker::spin_cb

(

)

Timer callback.

Definition at line 119 of file guidance_worker.cpp.

  {
    if(gsm_.shouldCallSetEnableRobotic()) {
      auto req = std::make_shared<carma_driver_msgs::srv::SetEnableRobotic::Request>();
      req->set = carma_driver_msgs::srv::SetEnableRobotic::Request::ENABLE;
      enable_client_->async_send_request(req);
    }
 
    carma_planning_msgs::msg::GuidanceState state;
    state.state = gsm_.getCurrentState();
    state_publisher_->publish(state);
    return true;
  }

References enable_client_, guidance::GuidanceStateMachine::getCurrentState(), gsm_, guidance::GuidanceStateMachine::shouldCallSetEnableRobotic(), and state_publisher_.

Referenced by handle_on_activate().

Here is the call graph for this function:

Here is the caller graph for this function:

vehicle_status_cb()

void guidance::GuidanceWorker::vehicle_status_cb

(

const autoware_msgs::msg::VehicleStatus::UniquePtr

msg

)

Callback for vehicle status messages.

Parameters

msg	Latest vehicle status message

Definition at line 143 of file guidance_worker.cpp.

  {
    gsm_.onVehicleStatus(move(msg));
  }

References gsm_, and guidance::GuidanceStateMachine::onVehicleStatus().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

config_

Config guidance::GuidanceWorker::config_

private

Definition at line 61 of file guidance_worker.hpp.

Referenced by GuidanceWorker(), handle_on_activate(), handle_on_configure(), and parameter_update_callback().

enable_client_

carma_ros2_utils::ClientPtr<carma_driver_msgs::srv::SetEnableRobotic> guidance::GuidanceWorker::enable_client_

private

Definition at line 52 of file guidance_worker.hpp.

Referenced by guidance_activation_cb(), handle_on_configure(), and spin_cb().

gsm_

GuidanceStateMachine guidance::GuidanceWorker::gsm_

private

Definition at line 64 of file guidance_worker.hpp.

Referenced by guidance_activation_cb(), handle_on_activate(), handle_on_shutdown(), robot_status_cb(), route_event_cb(), spin_cb(), and vehicle_status_cb().

guidance_activate_service_server_

carma_ros2_utils::ServicePtr<carma_planning_msgs::srv::SetGuidanceActive> guidance::GuidanceWorker::guidance_activate_service_server_

private

Definition at line 55 of file guidance_worker.hpp.

Referenced by handle_on_configure().

robot_status_subscriber_

carma_ros2_utils::SubPtr<carma_driver_msgs::msg::RobotEnabled> guidance::GuidanceWorker::robot_status_subscriber_

private

Definition at line 44 of file guidance_worker.hpp.

Referenced by handle_on_configure().

route_event_subscriber_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::RouteEvent> guidance::GuidanceWorker::route_event_subscriber_

private

Definition at line 45 of file guidance_worker.hpp.

Referenced by handle_on_configure().

spin_timer_

rclcpp::TimerBase::SharedPtr guidance::GuidanceWorker::spin_timer_

private

Definition at line 58 of file guidance_worker.hpp.

Referenced by handle_on_activate().

state_publisher_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::GuidanceState> guidance::GuidanceWorker::state_publisher_

private

Definition at line 49 of file guidance_worker.hpp.

Referenced by handle_on_configure(), and spin_cb().

vehicle_status_subscriber_

carma_ros2_utils::SubPtr<autoware_msgs::msg::VehicleStatus> guidance::GuidanceWorker::vehicle_status_subscriber_

private

Definition at line 46 of file guidance_worker.hpp.

Referenced by handle_on_configure().

---

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

• guidance/include/guidance/guidance_worker.hpp
• guidance/src/guidance_worker.cpp