#include <gnss_to_map_convertor_node.hpp>

Inheritance diagram for gnss_to_map_convertor::Node:

Collaboration diagram for gnss_to_map_convertor::Node:

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

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

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

Public Attributes
carma_ros2_utils::PubPtr< geometry_msgs::msg::PoseStamped >	map_pose_pub

carma_ros2_utils::SubPtr< gps_msgs::msg::GPSFix >	fix_sub_

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

Private Attributes
Config	config_

std::shared_ptr< tf2_ros::Buffer >	tfBuffer_

std::shared_ptr< tf2_ros::TransformListener >	tfListener_

std::shared_ptr< GNSSToMapConvertor >	convertor_worker_

Detailed Description

Definition at line 46 of file gnss_to_map_convertor_node.hpp.

Constructor & Destructor Documentation

◆ Node()

gnss_to_map_convertor::Node::Node ( const rclcpp::NodeOptions & options )

explicit

Node constructor.

Definition at line 22 of file gnss_to_map_convertor_node.cpp.

                                            : carma_ros2_utils::CarmaLifecycleNode(options)
  {
    // Initialize tf buffer with clock and duration
    tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
    // Initialize transform listener with buffer
    tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
 
    // Create initial config
    config_ = Config();
 
    // Declare parameters
    config_.base_link_frame = declare_parameter<std::string>("base_link_frame", config_.base_link_frame);
    config_.map_frame = declare_parameter<std::string>("map_frame", config_.map_frame);
    config_.heading_frame = declare_parameter<std::string>("heading_frame", config_.heading_frame);
  }

References gnss_to_map_convertor::Config::base_link_frame, config_, gnss_to_map_convertor::Config::heading_frame, gnss_to_map_convertor::Config::map_frame, tfBuffer_, and tfListener_.

Member Function Documentation

◆ handle_on_configure()

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

Definition at line 49 of file gnss_to_map_convertor_node.cpp.

  {
    // Reset config
    config_ = Config();
 
    // Load parameters
    get_parameter<std::string>("base_link_frame", config_.base_link_frame);
    get_parameter<std::string>("map_frame", config_.map_frame);
    get_parameter<std::string>("heading_frame", config_.heading_frame);
 
    // Register runtime parameter update callback
    add_on_set_parameters_callback(std::bind(&Node::parameter_update_callback, this, std_ph::_1));
 
    // Map pose publisher
    map_pose_pub = create_publisher<geometry_msgs::msg::PoseStamped>("gnss_pose", 10);
 
    // Initialize primary worker object 
    convertor_worker_ = std::make_shared<GNSSToMapConvertor>(
        [this](const auto& msg) { map_pose_pub->publish(msg); },  // Lambda representing publication
 
        [this](const auto& target, const auto& source) -> boost::optional<geometry_msgs::msg::TransformStamped> {  // Lambda representing transform lookup
          geometry_msgs::msg::TransformStamped tf;
          try
          {
            tf = tfBuffer_->lookupTransform(target, source, rclcpp::Time(1, 0));
          }
          catch (tf2::TransformException& ex)
          {
            RCLCPP_ERROR_STREAM(get_logger(),"Could not lookup transform with exception " << ex.what());
            return boost::none;
          }
          return tf;
        },
 
        config_.map_frame, config_.base_link_frame, config_.heading_frame, this->get_node_logging_interface());
 
    // Fix Subscriber
 
    fix_sub_ = create_subscription<gps_msgs::msg::GPSFix>("gnss_fix_fused", 2,
                                                          std::bind(&GNSSToMapConvertor::gnssFixCb, convertor_worker_.get(), std_ph::_1));
 
    // Georeference subsciber
 
    geo_sub = create_subscription<std_msgs::msg::String>("georeference", 1,
              std::bind(&GNSSToMapConvertor::geoReferenceCallback, convertor_worker_.get(), std_ph::_1));
    
                            
    // Return success if everthing initialized successfully
    return CallbackReturn::SUCCESS;
  }

References gnss_to_map_convertor::Config::base_link_frame, config_, convertor_worker_, fix_sub_, geo_sub, gnss_to_map_convertor::GNSSToMapConvertor::geoReferenceCallback(), gnss_to_map_convertor::GNSSToMapConvertor::gnssFixCb(), gnss_to_map_convertor::Config::heading_frame, gnss_to_map_convertor::Config::map_frame, map_pose_pub, parameter_update_callback(), and tfBuffer_.

Here is the call graph for this function:

◆ parameter_update_callback()

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

Definition at line 38 of file gnss_to_map_convertor_node.cpp.

  {
    auto error = update_params<std::string>({{"base_link_frame", config_.base_link_frame},{"map_frame", config_.map_frame},{"heading_frame", config_.heading_frame}}, parameters);
 
    rcl_interfaces::msg::SetParametersResult result;
 
    result.successful = !error;
 
    return result;
  }