The class responsible for publishing MobilityPath messages based on the latest trajectory plan. More...

#include <mobilitypath_publisher.hpp>

Inheritance diagram for mobilitypath_publisher::MobilityPathPublication:

Collaboration diagram for mobilitypath_publisher::MobilityPathPublication:

Public Member Functions
	MobilityPathPublication (const rclcpp::NodeOptions &)
	MobilityPathPublication constructor. More...

rcl_interfaces::msg::SetParametersResult	parameter_update_callback (const std::vector< rclcpp::Parameter > &parameters)
	Function callback for dynamic parameter updates. More...

carma_v2x_msgs::msg::MobilityPath	mobility_path_message_generator (const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan)
	Generates a MobilityPath message from a TrajectoryPlan. More...

void	georeference_cb (const std_msgs::msg::String::UniquePtr msg)
	Callback for map projection string to define lat/lon -> map conversion. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)

Private Member Functions
bool	spin_callback ()
	Spin callback, which will be called frequently based on the configured spin rate. More...

void	trajectory_cb (const carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg)
	Callback for the trajectory plan subscriber, which stores the latest trajectory plan locally and stores its corresponding MobilityPath locally. More...

void	bsm_cb (const carma_v2x_msgs::msg::BSM::UniquePtr msg)
	Callback for the BSM subscriber, which stores the BSM's BSMCoreData locally. More...

void	guidance_state_cb (const carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Callback for the Guidance State. More...

carma_v2x_msgs::msg::MobilityHeader	compose_mobility_header (uint64_t time)
	Generates a MobilityHeader to be used for a published MobilityPath message. More...

carma_v2x_msgs::msg::Trajectory	trajectory_plan_to_trajectory (const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &traj_points) const
	Converts a Trajectory Plan to a (Mobility) Trajectory. More...

carma_v2x_msgs::msg::LocationECEF	trajectory_point_to_ECEF (const carma_planning_msgs::msg::TrajectoryPlanPoint &traj_point) const
	Converts Trajectory Plan Point to ECEF (accepts meters and outputs in cm) More...

Private Attributes
carma_ros2_utils::SubPtr< carma_planning_msgs::msg::TrajectoryPlan >	traj_sub_

carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM >	bsm_sub_

carma_ros2_utils::SubPtr< std_msgs::msg::String >	georeference_sub_

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::GuidanceState >	guidance_state_sub_

bool	guidance_engaged_ = false

carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityPath >	path_pub_

rclcpp::TimerBase::SharedPtr	path_pub_timer_

Config	config_

carma_planning_msgs::msg::TrajectoryPlan	latest_trajectory_

carma_v2x_msgs::msg::MobilityPath	latest_mobility_path_

carma_v2x_msgs::msg::BSMCoreData	bsm_core_

std::string	georeference_ {""}

std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_

std::string	recipient_id = ""

std::string	sender_bsm_id = "FFFF"

Detailed Description

The class responsible for publishing MobilityPath messages based on the latest trajectory plan.

Definition at line 44 of file mobilitypath_publisher.hpp.

Constructor & Destructor Documentation

◆ MobilityPathPublication()

mobilitypath_publisher::MobilityPathPublication::MobilityPathPublication ( const rclcpp::NodeOptions & options )

explicit

MobilityPathPublication constructor.

Definition at line 22 of file mobilitypath_publisher.cpp.

      : carma_ros2_utils::CarmaLifecycleNode(options)
  {
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.path_pub_rate = declare_parameter<double>("path_pub_rate", config_.path_pub_rate);
    config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
  }

References config_, mobilitypath_publisher::Config::path_pub_rate, and mobilitypath_publisher::Config::vehicle_id.

Member Function Documentation

◆ bsm_cb()

void mobilitypath_publisher::MobilityPathPublication::bsm_cb ( const carma_v2x_msgs::msg::BSM::UniquePtr msg )

private

Callback for the BSM subscriber, which stores the BSM's BSMCoreData locally.

Parameters

msg	Latest BSM message

Definition at line 122 of file mobilitypath_publisher.cpp.

  {
    bsm_core_ = msg->core_data;
  }

References bsm_core_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ compose_mobility_header()

carma_v2x_msgs::msg::MobilityHeader mobilitypath_publisher::MobilityPathPublication::compose_mobility_header ( uint64_t time )

private

Generates a MobilityHeader to be used for a published MobilityPath message.

Parameters

time	Time in milliseconds

Returns: A MobilityHeader

Definition at line 145 of file mobilitypath_publisher.cpp.

  {
    carma_v2x_msgs::msg::MobilityHeader header;
    header.sender_id = config_.vehicle_id;
    header.recipient_id = recipient_id;
    header.sender_bsm_id = BSMHelper::BSMHelper::bsmIDtoString(bsm_core_.id);
 
    // random GUID that identifies this particular plan for future reference
    header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
    header.timestamp = time; //time in millisecond
 
    return header;
  }

References bsm_core_, config_, recipient_id, carma_cooperative_perception::to_string(), and mobilitypath_publisher::Config::vehicle_id.

Referenced by mobility_path_message_generator().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ georeference_cb()

void mobilitypath_publisher::MobilityPathPublication::georeference_cb ( const std_msgs::msg::String::UniquePtr msg )

Callback for map projection string to define lat/lon -> map conversion.

msg The proj string defining the projection.

Definition at line 106 of file mobilitypath_publisher.cpp.

  {
    // Build projector from proj string
    if (georeference_ != msg->data)
    {
      georeference_ = msg->data;
      map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
    }
  }

References georeference_, and map_projector_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ guidance_state_cb()

void mobilitypath_publisher::MobilityPathPublication::guidance_state_cb ( const carma_planning_msgs::msg::GuidanceState::UniquePtr msg )

private

Callback for the Guidance State.

Parameters

msg	Latest GuidanceState message

Definition at line 80 of file mobilitypath_publisher.cpp.

  {
    guidance_engaged_ = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED);
  }

References lightbar_manager::ENGAGED, and guidance_engaged_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn mobilitypath_publisher::MobilityPathPublication::handle_on_activate ( const rclcpp_lifecycle::State & prev_state )

Definition at line 85 of file mobilitypath_publisher.cpp.

  {
    // Timer setup
    int path_pub_period_millisecs = (1 / config_.path_pub_rate) * 1000; // Conversion from frequency (Hz) to milliseconds time period
    path_pub_timer_ = create_timer(get_clock(),
                                   std::chrono::milliseconds(path_pub_period_millisecs),
                                   std::bind(&MobilityPathPublication::spin_callback, this));
 
    return CallbackReturn::SUCCESS;
  }

References config_, mobilitypath_publisher::Config::path_pub_rate, path_pub_timer_, and spin_callback().

Here is the call graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn mobilitypath_publisher::MobilityPathPublication::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 45 of file mobilitypath_publisher.cpp.

  {
    RCLCPP_INFO_STREAM(this->get_logger(), "MobilityPathPublication trying to configure");
 
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<double>("path_pub_rate", config_.path_pub_rate);
    get_parameter<std::string>("vehicle_id", config_.vehicle_id);
 
    RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&MobilityPathPublication::parameter_update_callback, this, std_ph::_1));
 
    // Setup subscribers
    traj_sub_ = create_subscription<carma_planning_msgs::msg::TrajectoryPlan>("plan_trajectory", 5,
                                                                               std::bind(&MobilityPathPublication::trajectory_cb, this, std_ph::_1));
    bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
                                                             std::bind(&MobilityPathPublication::bsm_cb, this, std_ph::_1));
 
    guidance_state_sub_ = create_subscription<carma_planning_msgs::msg::GuidanceState>("guidance_state", 5, std::bind(&MobilityPathPublication::guidance_state_cb, this, std::placeholders::_1));
 
    georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 1,
                                                                   std::bind(&MobilityPathPublication::georeference_cb, this, std_ph::_1));
 
    // Setup publishers
    path_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityPath>("mobility_path_msg", 5);
 
 
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

References bsm_cb(), bsm_sub_, config_, georeference_cb(), georeference_sub_, guidance_state_cb(), guidance_state_sub_, parameter_update_callback(), path_pub_, mobilitypath_publisher::Config::path_pub_rate, traj_sub_, trajectory_cb(), and mobilitypath_publisher::Config::vehicle_id.

Here is the call graph for this function:

◆ mobility_path_message_generator()

carma_v2x_msgs::msg::MobilityPath mobilitypath_publisher::MobilityPathPublication::mobility_path_message_generator ( const carma_planning_msgs::msg::TrajectoryPlan & trajectory_plan )

Generates a MobilityPath message from a TrajectoryPlan.

Parameters

trajectory_plan A TrajectoryPlan

Returns: A MobilityPath message

Definition at line 127 of file mobilitypath_publisher.cpp.

  {
    carma_v2x_msgs::msg::MobilityPath mobility_path_msg;
    // TODO this caluclation uses a poor assumption of zero latency see https://github.com/usdot-fhwa-stol/carma-platform/issues/1606
    uint64_t millisecs = get_clock()->now().nanoseconds() / 1000000;
    mobility_path_msg.m_header = compose_mobility_header(millisecs);
 
    if (!map_projector_) {
      RCLCPP_ERROR_STREAM(this->get_logger(), "MobilityPath cannot be populated as map projection is not available");
      return mobility_path_msg;
    }
 
    carma_v2x_msgs::msg::Trajectory mob_path_traj = trajectory_plan_to_trajectory(trajectory_plan.trajectory_points);
    mobility_path_msg.trajectory = mob_path_traj;
 
    return mobility_path_msg;
  }

References compose_mobility_header(), map_projector_, and trajectory_plan_to_trajectory().

Referenced by trajectory_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ parameter_update_callback()

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

Function callback for dynamic parameter updates.

Definition at line 33 of file mobilitypath_publisher.cpp.

  {
    auto error = update_params<double>({{"path_pub_rate", config_.path_pub_rate}}, parameters);
    auto error_2 = update_params<std::string>({{"vehicle_id", config_.vehicle_id}}, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error && !error_2;
 
    return result;
  }

References config_, mobilitypath_publisher::Config::path_pub_rate, and mobilitypath_publisher::Config::vehicle_id.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ spin_callback()

bool mobilitypath_publisher::MobilityPathPublication::spin_callback ( )

private

Spin callback, which will be called frequently based on the configured spin rate.

Definition at line 96 of file mobilitypath_publisher.cpp.

  {
    // update timestamp of mobilitypath
    uint64_t millisecs = get_clock()->now().nanoseconds() / 1000000;
    latest_mobility_path_.m_header.timestamp = millisecs; //time in millisecond
    if (guidance_engaged_)
      path_pub_->publish(latest_mobility_path_);
    return true;
  }

References guidance_engaged_, latest_mobility_path_, and path_pub_.

Referenced by handle_on_activate().

Here is the caller graph for this function:

◆ trajectory_cb()

void mobilitypath_publisher::MobilityPathPublication::trajectory_cb ( const carma_planning_msgs::msg::TrajectoryPlan::UniquePtr msg )

private

Callback for the trajectory plan subscriber, which stores the latest trajectory plan locally and stores its corresponding MobilityPath locally.

Parameters

msg	Latest trajectory plan message

Definition at line 116 of file mobilitypath_publisher.cpp.

  {
    latest_trajectory_ = *msg;
    latest_mobility_path_ = mobility_path_message_generator(latest_trajectory_);
  }

References latest_mobility_path_, latest_trajectory_, and mobility_path_message_generator().

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ trajectory_plan_to_trajectory()

carma_v2x_msgs::msg::Trajectory mobilitypath_publisher::MobilityPathPublication::trajectory_plan_to_trajectory ( const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > & traj_points ) const

private

Converts a Trajectory Plan to a (Mobility) Trajectory.

Parameters

traj_points The TrajectoryPlan to be converted into a (Mobility) Trajectory

Returns: A (Mobility) Trajectory

Definition at line 159 of file mobilitypath_publisher.cpp.

  {
    carma_v2x_msgs::msg::Trajectory traj;
 
    if (traj_points.empty()) {
      throw std::invalid_argument("Received an empty vector of Trajectory Plan Points");
    }
 
    carma_v2x_msgs::msg::LocationECEF ecef_location = trajectory_point_to_ECEF(traj_points[0]); //m to cm to fit the msg standard
 
    if (traj_points.size() < 2){
      RCLCPP_WARN_STREAM(this->get_logger(), "Received Trajectory Plan is too small");
      traj.offsets = {};
    }
    else{
      carma_v2x_msgs::msg::LocationECEF prev_point = ecef_location;
      for (size_t i=1; i<traj_points.size(); i++){
 
        carma_v2x_msgs::msg::LocationOffsetECEF offset;
        carma_v2x_msgs::msg::LocationECEF new_point = trajectory_point_to_ECEF(traj_points[i]); //m to cm to fit the msg standard
        offset.offset_x = (int16_t)(new_point.ecef_x - prev_point.ecef_x);
        offset.offset_y = (int16_t)(new_point.ecef_y - prev_point.ecef_y);
        offset.offset_z = (int16_t)(new_point.ecef_z - prev_point.ecef_z);
        prev_point = new_point;
        traj.offsets.push_back(offset);
        if( i >= 60 ){ break;};
      }
    }
 
    traj.location = ecef_location;
 
    return traj;
  }

References process_bag::i, and trajectory_point_to_ECEF().

Referenced by mobility_path_message_generator().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ trajectory_point_to_ECEF()

carma_v2x_msgs::msg::LocationECEF mobilitypath_publisher::MobilityPathPublication::trajectory_point_to_ECEF ( const carma_planning_msgs::msg::TrajectoryPlanPoint & traj_point ) const

private

Converts Trajectory Plan Point to ECEF (accepts meters and outputs in cm)

Parameters

traj_points The Trajectory Plan Point to be converted to ECEF

Returns: The Trajectory Plan Point in ECEF

Definition at line 193 of file mobilitypath_publisher.cpp.

  {
    if (!map_projector_) {
      throw std::invalid_argument("No map projector available for ecef conversion");
    }
    carma_v2x_msgs::msg::LocationECEF location;
 
    lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({traj_point.x, traj_point.y, 0.0}, 1);
    location.ecef_x = ecef_point.x() * 100.0;
    location.ecef_y = ecef_point.y() * 100.0;
    location.ecef_z = ecef_point.z() * 100.0;
 
    return location;
  }

References map_projector_.

Referenced by trajectory_plan_to_trajectory().

Here is the caller graph for this function:

Member Data Documentation

◆ bsm_core_

carma_v2x_msgs::msg::BSMCoreData mobilitypath_publisher::MobilityPathPublication::bsm_core_

private

Definition at line 72 of file mobilitypath_publisher.hpp.

Referenced by bsm_cb(), and compose_mobility_header().

◆ bsm_sub_

carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> mobilitypath_publisher::MobilityPathPublication::bsm_sub_

private

Definition at line 50 of file mobilitypath_publisher.hpp.

Referenced by handle_on_configure().

◆ config_

Config mobilitypath_publisher::MobilityPathPublication::config_

private

Definition at line 63 of file mobilitypath_publisher.hpp.

Referenced by MobilityPathPublication(), compose_mobility_header(), handle_on_activate(), handle_on_configure(), and parameter_update_callback().

◆ georeference_

std::string mobilitypath_publisher::MobilityPathPublication::georeference_ {""}

private

Definition at line 75 of file mobilitypath_publisher.hpp.

Referenced by georeference_cb().

◆ georeference_sub_

carma_ros2_utils::SubPtr<std_msgs::msg::String> mobilitypath_publisher::MobilityPathPublication::georeference_sub_

private

Definition at line 51 of file mobilitypath_publisher.hpp.

Referenced by handle_on_configure().

◆ guidance_engaged_

bool mobilitypath_publisher::MobilityPathPublication::guidance_engaged_ = false

private

Definition at line 54 of file mobilitypath_publisher.hpp.

Referenced by guidance_state_cb(), and spin_callback().

◆ guidance_state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> mobilitypath_publisher::MobilityPathPublication::guidance_state_sub_

private

Definition at line 52 of file mobilitypath_publisher.hpp.

Referenced by handle_on_configure().

◆ latest_mobility_path_

carma_v2x_msgs::msg::MobilityPath mobilitypath_publisher::MobilityPathPublication::latest_mobility_path_

private

Definition at line 69 of file mobilitypath_publisher.hpp.

Referenced by spin_callback(), and trajectory_cb().

◆ latest_trajectory_

carma_planning_msgs::msg::TrajectoryPlan mobilitypath_publisher::MobilityPathPublication::latest_trajectory_

private

Definition at line 66 of file mobilitypath_publisher.hpp.

Referenced by trajectory_cb().

◆ map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> mobilitypath_publisher::MobilityPathPublication::map_projector_

private

Definition at line 76 of file mobilitypath_publisher.hpp.

Referenced by georeference_cb(), mobility_path_message_generator(), and trajectory_point_to_ECEF().

◆ path_pub_

carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityPath> mobilitypath_publisher::MobilityPathPublication::path_pub_

private

Definition at line 57 of file mobilitypath_publisher.hpp.

Referenced by handle_on_configure(), and spin_callback().

◆ path_pub_timer_

rclcpp::TimerBase::SharedPtr mobilitypath_publisher::MobilityPathPublication::path_pub_timer_

private

Definition at line 60 of file mobilitypath_publisher.hpp.

Referenced by handle_on_activate().

◆ recipient_id

std::string mobilitypath_publisher::MobilityPathPublication::recipient_id = ""

private

Definition at line 79 of file mobilitypath_publisher.hpp.

Referenced by compose_mobility_header().

◆ sender_bsm_id

std::string mobilitypath_publisher::MobilityPathPublication::sender_bsm_id = "FFFF"

private

Definition at line 82 of file mobilitypath_publisher.hpp.

◆ traj_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::TrajectoryPlan> mobilitypath_publisher::MobilityPathPublication::traj_sub_

private

Definition at line 49 of file mobilitypath_publisher.hpp.

Referenced by handle_on_configure().

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

mobilitypath_publisher/include/mobilitypath_publisher/mobilitypath_publisher.hpp
mobilitypath_publisher/src/mobilitypath_publisher.cpp

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ MobilityPathPublication()

Member Function Documentation

◆ bsm_cb()

◆ compose_mobility_header()

◆ georeference_cb()

◆ guidance_state_cb()

◆ handle_on_activate()

◆ handle_on_configure()

◆ mobility_path_message_generator()

◆ parameter_update_callback()

◆ spin_callback()

◆ trajectory_cb()

◆ trajectory_plan_to_trajectory()

◆ trajectory_point_to_ECEF()

Member Data Documentation

◆ bsm_core_

◆ bsm_sub_

◆ config_

◆ georeference_

◆ georeference_sub_

◆ guidance_engaged_

◆ guidance_state_sub_

◆ latest_mobility_path_

◆ latest_trajectory_

◆ map_projector_

◆ path_pub_

◆ path_pub_timer_

◆ recipient_id

◆ sender_bsm_id

◆ traj_sub_