Merge pull request #159 from KumarRobotics/devel/humble_pranav

kr_mesh_visualization migration to ROS2 humble

kr_mesh_visualization/CMakeLists.txt

@@ -1,34 +1,41 @@
 cmake_minimum_required(VERSION 3.10)
 project(kr_mesh_visualization)
 
-# set default build type
-if(NOT CMAKE_BUILD_TYPE)
-  set(CMAKE_BUILD_TYPE RelWithDebInfo)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-set(CMAKE_CXX_STANDARD 14)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
-set(CMAKE_CXX_EXTENSIONS OFF)
-add_compile_options(-Wall)
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(visualization_msgs REQUIRED)
 
-find_package(catkin REQUIRED COMPONENTS geometry_msgs nav_msgs topic_tools visualization_msgs)
+add_executable(${PROJECT_NAME} src/mesh_visualization.cpp)
+ament_target_dependencies(${PROJECT_NAME} rclcpp geometry_msgs nav_msgs visualization_msgs)
 
-catkin_package(
-  CATKIN_DEPENDS
-  geometry_msgs
-  nav_msgs
-  topic_tools
-  visualization_msgs)
+install(DIRECTORY
+  mesh
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
 
-add_executable(${PROJECT_NAME} src/mesh_visualization.cpp)
-target_include_directories(${PROJECT_NAME} PRIVATE ${catkin_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} PRIVATE ${catkin_LIBRARIES})
+install(TARGETS
+  ${PROJECT_NAME}
+  DESTINATION lib/${PROJECT_NAME}
+)
 
-install(
-  TARGETS ${PROJECT_NAME}
-  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
 
-install(DIRECTORY launch/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)
-install(DIRECTORY mesh/ DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mesh)
+ament_package()

kr_mesh_visualization/README.md

@@ -0,0 +1,10 @@
+kr_mesh_visualization
+=============
+
+Package to visualize the drone in RViz.
+
+## Message types supported:
+
+Three message types are supported in this package in order to subscribe to the pose data and visualize the drone. The default message type that is being subscribed to is `nav_msgs/msg/Odometry`. The other message types supported are `geometry_msgs/msg/PoseStamped` and `geometry_msgs/msg/PoseWithCovarianceStamped`. ROS2 does not support multiple subscribers to the same topic with different message type.
+
+In order to change the message type for the subscriber, you can use the parameter `msg_type` to change the message type to any one of the 3 supported types. This parameter accepts a string of the message types listed above. 

kr_mesh_visualization/launch/test_launch.py

@@ -0,0 +1,46 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
+
+def generate_launch_description():
+
+  input_topic_launch_arg = DeclareLaunchArgument(
+    "~/input",
+    default_value=TextSubstitution(text="/quadrotor/odom")
+  )
+
+  new_frame_id_launch_arg = DeclareLaunchArgument(
+    "new_frame_id", default_value=TextSubstitution(text="")
+  )
+
+  msg_type_launch_arg = DeclareLaunchArgument(
+    "msg_type",
+    default_value=TextSubstitution(text="nav_msgs/msg/Odometry")
+  )
+
+  mesh_visualization_node = Node(
+    package="kr_mesh_visualization",
+    executable="kr_mesh_visualization",
+    name="mesh_visualization",
+    output="screen",
+    remappings=[
+      ("~/input", LaunchConfiguration("~/input"))
+    ],
+    parameters=[
+      {"mesh_resource": "package://kr_mesh_visualization/mesh/hummingbird.mesh"},
+      {"color/r": 0.0},
+      {"color/g": 0.0},
+      {"color/b": 1.0},
+      {"color/a": 0.7},
+      {"new_frame_id": LaunchConfiguration("new_frame_id")},
+      {"msg_type": LaunchConfiguration("msg_type")}
+    ]
+  )
+
+  return LaunchDescription([
+    input_topic_launch_arg,
+    new_frame_id_launch_arg,
+    msg_type_launch_arg,
+    mesh_visualization_node
+  ])

kr_mesh_visualization/package.xml

@@ -1,20 +1,25 @@
-<package format="2">
+<package format="3">
   <name>kr_mesh_visualization</name>
   <version>1.0.0</version>
   <description>kr_mesh_visualization</description>
-
+  
   <maintainer email="[email protected]">Kartik Mohta</maintainer>
   <maintainer email="[email protected]">Michael Watterson</maintainer>
-
+  <maintainer email="[email protected]">Pranav Shah</maintainer>
   <license>BSD</license>
-
   <author>Kartik Mohta</author>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>ament_cmake</buildtool_depend>
 
-  <depend>nav_msgs</depend>
+  <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
-  <depend>topic_tools</depend>
+  <depend>nav_msgs</depend>
   <depend>visualization_msgs</depend>
 
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>

kr_mesh_visualization/src/mesh_visualization.cpp

@@ -1,83 +1,114 @@
-#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/PoseWithCovarianceStamped.h>
-#include <nav_msgs/Odometry.h>
-#include <ros/ros.h>
-#include <topic_tools/shape_shifter.h>
-#include <visualization_msgs/Marker.h>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "visualization_msgs/msg/marker.hpp"
 
-static std::string mesh_resource, new_frame_id;
-static ros::Publisher pub_vis;
-static double color_r, color_g, color_b, color_a;
-static double scale_x, scale_y, scale_z;
-
-static void publishMarker(const std::string &frame_id, const geometry_msgs::Pose &pose)
+class MeshVisualizationNode : public rclcpp::Node
 {
-  visualization_msgs::Marker marker;
-  marker.header.frame_id = (new_frame_id == "") ? frame_id : new_frame_id;
-  marker.header.stamp = ros::Time();  // time 0 so that the marker will be
-                                      // displayed regardless of the current time
-  marker.ns = ros::this_node::getName();
-  marker.id = 0;
-  marker.type = visualization_msgs::Marker::MESH_RESOURCE;
-  marker.action = visualization_msgs::Marker::ADD;
-  marker.pose = pose;
-  marker.scale.x = scale_x;
-  marker.scale.y = scale_y;
-  marker.scale.z = scale_z;
-  marker.color.a = color_a;
-  marker.color.r = color_r;
-  marker.color.g = color_g;
-  marker.color.b = color_b;
-  marker.mesh_resource = mesh_resource;
-  pub_vis.publish(marker);
-}
+ public:
+  MeshVisualizationNode() : Node("kr_mesh_visualization")
+  {
+    this->declare_parameter<std::string>("mesh_resource",
+                                         std::string("package://kr_mesh_visualization/mesh/hummingbird.mesh"));
+    this->declare_parameter<double>("color/r", 1.0);
+    this->declare_parameter<double>("color/g", 0.0);
+    this->declare_parameter<double>("color/b", 0.0);
+    this->declare_parameter<double>("color/a", 1.0);
+    this->declare_parameter<double>("scale/x", 1.0);
+    this->declare_parameter<double>("scale/y", 1.0);
+    this->declare_parameter<double>("scale/z", 1.0);
+    this->declare_parameter<std::string>("new_frame_id", std::string(""));
+    this->declare_parameter<std::string>("msg_type", std::string("nav_msgs/msg/Odometry"));
 
-static void any_callback(const topic_tools::ShapeShifter::ConstPtr &msg)
-{
-  // ROS_INFO_STREAM("Type: " << msg->getDataType());
+    this->get_parameter("mesh_resource", mesh_resource);
+    this->get_parameter("color/r", color_r);
+    this->get_parameter("color/g", color_g);
+    this->get_parameter("color/b", color_b);
+    this->get_parameter("color/a", color_a);
+    this->get_parameter("scale/x", scale_x);
+    this->get_parameter("scale/y", scale_y);
+    this->get_parameter("scale/z", scale_z);
+    this->get_parameter("new_frame_id", new_frame_id);
+    this->get_parameter("msg_type", msg_type);
+
+    using namespace std::placeholders;
+    pub_vis_ = this->create_publisher<visualization_msgs::msg::Marker>("~/robot", 10);
 
-  if(msg->getDataType() == "geometry_msgs/PoseStamped")
+    if(msg_type == "nav_msgs/msg/Odometry")
+    {
+      sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
+          "~/input", 10, std::bind(&MeshVisualizationNode::odom_callback, this, _1));
+    }
+    else if(msg_type == "geometry_msgs/msg/PoseStamped")
+    {
+      sub_ps_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+          "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_stamped, this, _1));
+    }
+    else if(msg_type == "geometry_msgs/msg/PoseWithCovarianceStamped")
+    {
+      sub_pwcs_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+          "~/input", 10, std::bind(&MeshVisualizationNode::callback_pose_with_covariance_stamped, this, _1));
+    }
+    else
+    {
+      RCLCPP_ERROR_STREAM(this->get_logger(), std::string(this->get_name()) << " got unsupported message type " << msg_type << "\n");
+    }
+  }
+
+ protected:
+  std::string mesh_resource, new_frame_id;
+  rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr pub_vis_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_ps_;
+  rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_pwcs_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
+  double color_r, color_g, color_b, color_a;
+  double scale_x, scale_y, scale_z;
+  std::string msg_type;
+
+  void publishMarker(const std::string &frame_id, const geometry_msgs::msg::Pose &pose)
   {
-    auto pose_msg = msg->instantiate<geometry_msgs::PoseStamped>();
-    publishMarker(pose_msg->header.frame_id, pose_msg->pose);
+    auto marker = visualization_msgs::msg::Marker();
+
+    marker.header.frame_id = (new_frame_id == "") ? frame_id : new_frame_id;
+    marker.header.stamp = rclcpp::Time(0, 0);
+    marker.ns = this->get_name();
+    marker.id = 0;
+    marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE;
+    marker.action = visualization_msgs::msg::Marker::ADD;
+    marker.pose = pose;
+    marker.scale.x = scale_x;
+    marker.scale.y = scale_y;
+    marker.scale.z = scale_z;
+    marker.color.a = color_a;
+    marker.color.r = color_r;
+    marker.color.g = color_g;
+    marker.color.b = color_b;
+    marker.mesh_resource = mesh_resource;
+    pub_vis_->publish(marker);
   }
-  else if(msg->getDataType() == "geometry_msgs/PoseWithCovarianceStamped")
+
+  void callback_pose_stamped(const geometry_msgs::msg::PoseStamped::SharedPtr msg)
   {
-    auto pose_msg = msg->instantiate<geometry_msgs::PoseWithCovarianceStamped>();
-    publishMarker(pose_msg->header.frame_id, pose_msg->pose.pose);
+    publishMarker(msg->header.frame_id, msg->pose);
   }
-  else if(msg->getDataType() == "nav_msgs/Odometry")
+
+  void callback_pose_with_covariance_stamped(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
   {
-    auto odom_msg = msg->instantiate<nav_msgs::Odometry>();
-    publishMarker(odom_msg->header.frame_id, odom_msg->pose.pose);
+    publishMarker(msg->header.frame_id, msg->pose.pose);
   }
-  else
+
+  void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg)
   {
-    ROS_ERROR_STREAM(ros::this_node::getName() << " got unsupported message type " << msg->getDataType());
+    publishMarker(msg->header.frame_id, msg->pose.pose);
   }
-}
+};
 
 int main(int argc, char **argv)
 {
-  ros::init(argc, argv, "kr_mesh_visualization");
-
-  ros::NodeHandle nh("~");
-
-  nh.param("mesh_resource", mesh_resource, std::string("package://kr_mesh_visualization/mesh/hummingbird.mesh"));
-  nh.param("color/r", color_r, 1.0);
-  nh.param("color/g", color_g, 0.0);
-  nh.param("color/b", color_b, 0.0);
-  nh.param("color/a", color_a, 1.0);
-  nh.param("scale/x", scale_x, 1.0);
-  nh.param("scale/y", scale_y, 1.0);
-  nh.param("scale/z", scale_z, 1.0);
-
-  nh.param("new_frame_id", new_frame_id, std::string(""));
-
-  pub_vis = nh.advertise<visualization_msgs::Marker>("robot", 10);
-
-  ros::Subscriber any_sub = nh.subscribe("input", 10, &any_callback, ros::TransportHints().tcpNoDelay());
-  ros::spin();
-
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<MeshVisualizationNode>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
   return 0;
 }