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.
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
object::ObjectDetectionTrackingNode Class Reference

#include <object_detection_tracking_node.h>

Inheritance diagram for object::ObjectDetectionTrackingNode:
Inheritance graph
Collaboration diagram for object::ObjectDetectionTrackingNode:
Collaboration graph

Public Member Functions

 ObjectDetectionTrackingNode (const rclcpp::NodeOptions &)
 ObjectDetectionTrackingNode constructor. More...
 
void publishObject (const carma_perception_msgs::msg::ExternalObjectList &obj_msg)
 Callback to publish ObjectList. More...
 
boost::optional< geometry_msgs::msg::TransformStamped > lookupTransform (const std::string &parent, const std::string &child, const rclcpp::Time &stamp)
 Callback to lookup a transform between two frames. More...
 
carma_ros2_utils::CallbackReturn handle_on_configure (const rclcpp_lifecycle::State &)
 

Private Attributes

carma_ros2_utils::SubPtr< autoware_auto_msgs::msg::TrackedObjects > autoware_obj_sub_
 
carma_ros2_utils::PubPtr< carma_perception_msgs::msg::ExternalObjectList > carma_obj_pub_
 
ObjectDetectionTrackingWorker object_worker_
 
std::shared_ptr< tf2_ros::Buffer > tfBuffer_
 
std::shared_ptr< tf2_ros::TransformListener > tfListener_
 
std::string map_frame_
 

Detailed Description

Definition at line 31 of file object_detection_tracking_node.h.

Constructor & Destructor Documentation

◆ ObjectDetectionTrackingNode()

object::ObjectDetectionTrackingNode::ObjectDetectionTrackingNode ( const rclcpp::NodeOptions &  options)
explicit

ObjectDetectionTrackingNode constructor.

Definition at line 24 of file object_detection_tracking_node.cpp.

25 : carma_ros2_utils::CarmaLifecycleNode(options),
27 std::bind(&ObjectDetectionTrackingNode::publishObject, this, _1),
28 std::bind(&ObjectDetectionTrackingNode::lookupTransform, this, _1, _2, _3),
29 get_node_logging_interface()
30 )
31 {
32 // Initialize tf buffer with clock and duration
33 tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
34 // Initialize transform listener with buffer
35 tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
36 map_frame_ = this->declare_parameter<std::string>("map_frame", map_frame_);
37 }
void publishObject(const carma_perception_msgs::msg::ExternalObjectList &obj_msg)
Callback to publish ObjectList.
boost::optional< geometry_msgs::msg::TransformStamped > lookupTransform(const std::string &parent, const std::string &child, const rclcpp::Time &stamp)
Callback to lookup a transform between two frames.
std::shared_ptr< tf2_ros::TransformListener > tfListener_
std::shared_ptr< tf2_ros::Buffer > tfBuffer_

References map_frame_, tfBuffer_, and tfListener_.

Member Function Documentation

◆ handle_on_configure()

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

Definition at line 39 of file object_detection_tracking_node.cpp.

39 {
40
41 // Load parameters
42 this->get_parameter<std::string>("map_frame", map_frame_);
44
45 this->add_on_set_parameters_callback(
46 [this](auto param_vec) {
47
48 auto error = update_params<std::string>({ {"map_frame", map_frame_} }, param_vec);
49
50 rcl_interfaces::msg::SetParametersResult result;
51
52 result.successful = !error;
53
54 if (error) {
55 result.reason = error.get();
56 } else {
58 }
59
60 return result;
61 }
62 );
63
64
65 // Setup pub/sub
66 autoware_obj_sub_= create_subscription<autoware_auto_msgs::msg::TrackedObjects>("detected_objects",10,
68 );
69
70 carma_obj_pub_= create_publisher<carma_perception_msgs::msg::ExternalObjectList>("external_objects", 10);
71
72 return CallbackReturn::SUCCESS;
73 }
carma_ros2_utils::SubPtr< autoware_auto_msgs::msg::TrackedObjects > autoware_obj_sub_
carma_ros2_utils::PubPtr< carma_perception_msgs::msg::ExternalObjectList > carma_obj_pub_
void detectedObjectCallback(autoware_auto_msgs::msg::TrackedObjects::UniquePtr msg)
detectedObjectCallback populates detected object along with its velocity,yaw, yaw_rate and static/dyn...

References autoware_obj_sub_, carma_obj_pub_, object::ObjectDetectionTrackingWorker::detectedObjectCallback(), map_frame_, object_worker_, and object::ObjectDetectionTrackingWorker::setMapFrame().

Here is the call graph for this function:

◆ lookupTransform()

boost::optional< geometry_msgs::msg::TransformStamped > object::ObjectDetectionTrackingNode::lookupTransform ( const std::string &  parent,
const std::string &  child,
const rclcpp::Time &  stamp 
)

Callback to lookup a transform between two frames.

Parameters
parentThe parent frame
childThe child frame
stampThe time stamp of the transform
Returns
boost::optional<geometry_msgs::TransformStamped> The transform between the two frames or boost::none if the transform does not exist or cannot be computed

Definition at line 81 of file object_detection_tracking_node.cpp.

81 {
82 try {
83 return tfBuffer_->lookupTransform(parent, child, stamp);
84
85 } catch (tf2::TransformException &ex) {
86
87 RCLCPP_WARN_STREAM(get_logger(), "Failed to find transform with exception " << ex.what());
88 return boost::none;
89 }
90 }

References tfBuffer_.

◆ publishObject()

void object::ObjectDetectionTrackingNode::publishObject ( const carma_perception_msgs::msg::ExternalObjectList &  obj_msg)

Callback to publish ObjectList.

Definition at line 75 of file object_detection_tracking_node.cpp.

76 {
77 carma_obj_pub_->publish(obj_msg);
78 }

References carma_obj_pub_.

Member Data Documentation

◆ autoware_obj_sub_

carma_ros2_utils::SubPtr<autoware_auto_msgs::msg::TrackedObjects> object::ObjectDetectionTrackingNode::autoware_obj_sub_
private

Definition at line 37 of file object_detection_tracking_node.h.

Referenced by handle_on_configure().

◆ carma_obj_pub_

carma_ros2_utils::PubPtr<carma_perception_msgs::msg::ExternalObjectList> object::ObjectDetectionTrackingNode::carma_obj_pub_
private

Definition at line 40 of file object_detection_tracking_node.h.

Referenced by handle_on_configure(), and publishObject().

◆ map_frame_

std::string object::ObjectDetectionTrackingNode::map_frame_
private

◆ object_worker_

ObjectDetectionTrackingWorker object::ObjectDetectionTrackingNode::object_worker_
private

Definition at line 43 of file object_detection_tracking_node.h.

Referenced by handle_on_configure().

◆ tfBuffer_

std::shared_ptr<tf2_ros::Buffer> object::ObjectDetectionTrackingNode::tfBuffer_
private

◆ tfListener_

std::shared_ptr<tf2_ros::TransformListener> object::ObjectDetectionTrackingNode::tfListener_
private

Definition at line 49 of file object_detection_tracking_node.h.

Referenced by ObjectDetectionTrackingNode().


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