diff --git a/src/perception/radar_object_detection/radar_vis/include/radar_vis.hpp b/src/perception/radar_object_detection/radar_vis/include/radar_vis.hpp index 49b800b2..03fda5a5 100644 --- a/src/perception/radar_object_detection/radar_vis/include/radar_vis.hpp +++ b/src/perception/radar_object_detection/radar_vis/include/radar_vis.hpp @@ -2,23 +2,21 @@ #define RADAR_VIS_HPP_ #include + +#include "radar_msgs/msg/radar_detection.hpp" +#include "radar_msgs/msg/radar_packet.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/point_field.hpp" -#include "radar_msgs/msg/radar_packet.hpp" -#include "radar_msgs/msg/radar_detection.hpp" - -namespace visualization -{ +namespace visualization { -class RadarVis -{ -public: +class RadarVis { + public: RadarVis(); sensor_msgs::msg::PointCloud2 convert_packet_to_pointcloud( - const radar_msgs::msg::RadarPacket::SharedPtr msg); + const radar_msgs::msg::RadarPacket::SharedPtr msg); }; } // namespace visualization diff --git a/src/perception/radar_object_detection/radar_vis/include/radar_vis_node.hpp b/src/perception/radar_object_detection/radar_vis/include/radar_vis_node.hpp index 3ccd8fbf..003b0344 100644 --- a/src/perception/radar_object_detection/radar_vis/include/radar_vis_node.hpp +++ b/src/perception/radar_object_detection/radar_vis/include/radar_vis_node.hpp @@ -1,41 +1,42 @@ #ifndef RADAR_VIS_NODE_HPP_ #define RADAR_VIS_NODE_HPP_ +#include "radar_msgs/msg/radar_detection.hpp" +#include "radar_msgs/msg/radar_packet.hpp" +#include "radar_vis.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/point_field.hpp" -#include "radar_msgs/msg/radar_packet.hpp" -#include "radar_msgs/msg/radar_detection.hpp" - -#include "radar_vis.hpp" - /** -* @brief Implementation of a Radar visualization node that listens to "processed" topic -*/ -class RadarVisNode : public rclcpp::Node -{ -public: + * @brief Implementation of a Radar visualization node that listens to + * "processed" topic + */ +class RadarVisNode : public rclcpp::Node { + public: RadarVisNode(); /** - * @brief A ROS2 subscription node callback used to unpack filtered radar data from the "processed" - * topic. The callback listens to radar packets on this topic and processes them - * in order to visualize them in tools like foxglove and RViz. - * - * @param msg a raw message from the "processed" topic - */ + * @brief A ROS2 subscription node callback used to unpack filtered radar data + * from the "processed" topic. The callback listens to radar packets on this + * topic and processes them in order to visualize them in tools like foxglove + * and RViz. + * + * @param msg a raw message from the "processed" topic + */ void process_radar_data_callback( - const radar_msgs::msg::RadarPacket::SharedPtr msg); + const radar_msgs::msg::RadarPacket::SharedPtr msg); -private: + private: // ROS2 Subscriber listening to "processed" topic. rclcpp::Subscription::SharedPtr raw_sub_; - // ROS2 Publisher that sends point cloud data to "visualization" topic for foxglove/Rviz. + // ROS2 Publisher that sends point cloud data to "visualization" topic for + // foxglove/Rviz. rclcpp::Publisher::SharedPtr raw_pub_; - // An object containing methods for converting radar packets into point clouds. + // An object containing methods for converting radar packets into point + // clouds. visualization::RadarVis packet_converter_; }; diff --git a/src/perception/radar_object_detection/radar_vis/src/radar_vis.cpp b/src/perception/radar_object_detection/radar_vis/src/radar_vis.cpp index 62177b2a..114cbe09 100644 --- a/src/perception/radar_object_detection/radar_vis/src/radar_vis.cpp +++ b/src/perception/radar_object_detection/radar_vis/src/radar_vis.cpp @@ -1,19 +1,16 @@ -#include +#include "radar_vis.hpp" -#include "rclcpp/rclcpp.hpp" +#include #include "radar_vis_node.hpp" -#include "radar_vis.hpp" +#include "rclcpp/rclcpp.hpp" -namespace visualization -{ +namespace visualization { -RadarVis::RadarVis() -{} +RadarVis::RadarVis() {} sensor_msgs::msg::PointCloud2 RadarVis::convert_packet_to_pointcloud( - const radar_msgs::msg::RadarPacket::SharedPtr msg) -{ + const radar_msgs::msg::RadarPacket::SharedPtr msg) { std::string radar_frame = "radar_fixed"; sensor_msgs::msg::PointCloud2 point_cloud; sensor_msgs::msg::PointField point_field; @@ -50,7 +47,7 @@ sensor_msgs::msg::PointCloud2 RadarVis::convert_packet_to_pointcloud( point_cloud.fields.push_back(point_field); // Convert data to 4 bytes (little endian) - uint8_t * tmp_ptr; + uint8_t *tmp_ptr; for (uint8_t index = 0; index < msg->detections.size(); index++) { // Position X - Point Cloud Conversion tmp_ptr = reinterpret_cast(&(msg->detections[index].pos_x)); diff --git a/src/perception/radar_object_detection/radar_vis/src/radar_vis_node.cpp b/src/perception/radar_object_detection/radar_vis/src/radar_vis_node.cpp index 9289d01d..bfae531d 100644 --- a/src/perception/radar_object_detection/radar_vis/src/radar_vis_node.cpp +++ b/src/perception/radar_object_detection/radar_vis/src/radar_vis_node.cpp @@ -1,29 +1,27 @@ +#include "radar_vis_node.hpp" + #include -#include "radar_vis_node.hpp" #include "radar_vis.hpp" -RadarVisNode::RadarVisNode() -: Node("radar_vis") -{ +RadarVisNode::RadarVisNode() : Node("radar_vis") { raw_sub_ = this->create_subscription( - "processed", - 1, std::bind( - &RadarVisNode::process_radar_data_callback, - this, std::placeholders::_1)); - raw_pub_ = this->create_publisher("visualization", 20); + "processed", 1, + std::bind(&RadarVisNode::process_radar_data_callback, this, + std::placeholders::_1)); + raw_pub_ = this->create_publisher( + "visualization", 20); } void RadarVisNode::process_radar_data_callback( - const radar_msgs::msg::RadarPacket::SharedPtr msg) -{ + const radar_msgs::msg::RadarPacket::SharedPtr msg) { sensor_msgs::msg::PointCloud2 publish_packet_point_cloud; - publish_packet_point_cloud = packet_converter_.convert_packet_to_pointcloud(msg); + publish_packet_point_cloud = + packet_converter_.convert_packet_to_pointcloud(msg); raw_pub_->publish(publish_packet_point_cloud); } -int main(int argc, char ** argv) -{ +int main(int argc, char** argv) { rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); diff --git a/src/perception/radar_object_detection/radar_vis/test/radar_vis_test.cpp b/src/perception/radar_object_detection/radar_vis/test/radar_vis_test.cpp index 9530d437..619a384c 100644 --- a/src/perception/radar_object_detection/radar_vis/test/radar_vis_test.cpp +++ b/src/perception/radar_object_detection/radar_vis/test/radar_vis_test.cpp @@ -1,15 +1,13 @@ +#include "radar_vis.hpp" + #include #include #include "gtest/gtest.h" -#include "rclcpp/rclcpp.hpp" - -#include "radar_vis.hpp" #include "radar_vis_node.hpp" +#include "rclcpp/rclcpp.hpp" - -TEST(RadarVisNodeTest, CheckConvertPacketToPointCloud) -{ +TEST(RadarVisNodeTest, CheckConvertPacketToPointCloud) { visualization::RadarVis test_vis; auto msg = std::make_shared(); radar_msgs::msg::RadarDetection msg_detection; @@ -46,7 +44,8 @@ TEST(RadarVisNodeTest, CheckConvertPacketToPointCloud) EXPECT_EQ(test_point_cloud.fields[3].name, "intensity"); EXPECT_EQ(test_point_cloud.fields[3].offset, 12); - // Little Endian Conversion (32 bit float into 4 bytes; each 4 bytes represents one point field) + // Little Endian Conversion (32 bit float into 4 bytes; each 4 bytes + // represents one point field) // Position X EXPECT_EQ(test_point_cloud.data[0], 0); diff --git a/watod-config.sh b/watod-config.sh index 609febfa..38299dea 100755 --- a/watod-config.sh +++ b/watod-config.sh @@ -12,7 +12,7 @@ ## - simulation : starts simulation ## - samples : starts sample ROS2 pubsub nodes -# ACTIVE_MODULES="" +ACTIVE_MODULES="perception" ############################## OPTIONAL CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = ""