Update code for Humble

CMakeLists.txt

@@ -22,7 +22,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclpy REQUIRED)
 
 find_package(geometry_msgs REQUIRED)
-find_package(jackal_msgs REQUIRED)
+find_package(clearpath_platform_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(std_msgs REQUIRED)
@@ -47,7 +47,7 @@ target_include_directories(
 
 ament_target_dependencies(
   jackal_hardware
-  jackal_msgs
+  clearpath_platform_msgs
   hardware_interface
   pluginlib
   rclcpp

include/jackal_hardware/jackal_hardware.hpp

@@ -39,13 +39,11 @@
 #include <string>
 #include <vector>
 #include <chrono>
-
-#include "hardware_interface/base_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
-#include "hardware_interface/types/hardware_interface_status_values.hpp"
 #include "hardware_interface/visibility_control.h"
 
 #include "jackal_hardware_interface.hpp"
@@ -55,13 +53,13 @@ namespace jackal_hardware
 {
 
 class JackalHardware
-  : public hardware_interface::BaseInterface<hardware_interface::SystemInterface>
+  : public hardware_interface::SystemInterface
 {
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(JackalHardware)
 
   HARDWARE_INTERFACE_PUBLIC
-  hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
+  hardware_interface::CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
 
   HARDWARE_INTERFACE_PUBLIC
   std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
@@ -70,16 +68,16 @@ class JackalHardware
   std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
 
   HARDWARE_INTERFACE_PUBLIC
-  hardware_interface::return_type start() override;
+  hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
   HARDWARE_INTERFACE_PUBLIC
-  hardware_interface::return_type stop() override;
+  hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   HARDWARE_INTERFACE_PUBLIC
-  hardware_interface::return_type read() override;
+  hardware_interface::return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   HARDWARE_INTERFACE_PUBLIC
-  hardware_interface::return_type write() override;
+  hardware_interface::return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 private:
   void writeCommandsToHardware();

include/jackal_hardware/jackal_hardware_interface.hpp

@@ -39,8 +39,8 @@
 
 #include "rclcpp/rclcpp.hpp"
 
-#include "jackal_msgs/msg/drive.hpp"
-#include "jackal_msgs/msg/feedback.hpp"
+#include "clearpath_platform_msgs/msg/drive.hpp"
+#include "clearpath_platform_msgs/msg/feedback.hpp"
 
 namespace jackal_hardware
 {
@@ -51,15 +51,15 @@ class JackalHardwareInterface
   public:
   explicit JackalHardwareInterface();
   void drive_command(float left_wheel, float right_wheel, int8_t mode);
-  jackal_msgs::msg::Feedback get_feedback();
+  clearpath_platform_msgs::msg::Feedback get_feedback();
 
   private:
-  void feedback_callback(const jackal_msgs::msg::Feedback::SharedPtr msg);
+  void feedback_callback(const clearpath_platform_msgs::msg::Feedback::SharedPtr msg);
 
-  rclcpp::Publisher<jackal_msgs::msg::Drive>::SharedPtr drive_pub_;
-  rclcpp::Subscription<jackal_msgs::msg::Feedback>::SharedPtr feedback_sub_;
+  rclcpp::Publisher<clearpath_platform_msgs::msg::Drive>::SharedPtr drive_pub_;
+  rclcpp::Subscription<clearpath_platform_msgs::msg::Feedback>::SharedPtr feedback_sub_;
 
-  jackal_msgs::msg::Feedback feedback_;
+  clearpath_platform_msgs::msg::Feedback feedback_;
   std::mutex feedback_mutex_;
 };
 

jackal_robot/diagnostics_updater

@@ -40,7 +40,7 @@ import rclpy
 from rclpy.node import Node
 from rclpy.qos import qos_profile_sensor_data
 
-from jackal_msgs.msg import Status, StopStatus, Power, Feedback
+from clearpath_platform_msgs.msg import Status, StopStatus, Power, Feedback
 
 from nmea_msgs.msg import Sentence
 

launch/accessories.launch.py

@@ -4,6 +4,9 @@
 from launch.substitutions import EnvironmentVariable, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution
 
 
 def generate_launch_description():
@@ -31,6 +34,18 @@ def generate_launch_description():
                 ]
             )
             ld.add_action(node_hokuyo)
+        elif (primary_lidar_model.perform(lc) == 'femto_mega'):
+            femto_mega = IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    PathJoinSubstitution(
+                        [FindPackageShare('orbbec_camera'),
+                        'launch',
+                        'femto_mega.launch.py']
+                    )
+                )
+            )
+
+            ld.add_action(femto_mega)
 
 
     # Secondary Lidar Environment Variables

launch/bringup.launch.py

@@ -70,19 +70,19 @@ def generate_launch_description():
         ),
 
         # Wireless Watcher
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(PathJoinSubstitution(
-                [FindPackageShare('wireless_watcher'), 'launch', 'watcher.launch.py']
-            )),
-            launch_arguments=[('connected_topic', 'wifi_connected')]
-        ),
+        #IncludeLaunchDescription(
+        #    PythonLaunchDescriptionSource(PathJoinSubstitution(
+        #        [FindPackageShare('wireless_watcher'), 'launch', 'watcher.launch.py']
+        #    )),
+        #    launch_arguments=[('connected_topic', 'wifi_connected')]
+        #),
 
         # MicroROS Agent
         Node(
             package='micro_ros_agent',
             executable='micro_ros_agent',
             arguments=['serial', '--dev', '/dev/jackal'],
-            output='screen',
+            output='screen'
         ),
     ])
 

package.xml

@@ -17,7 +17,7 @@
   <depend>diagnostic_updater</depend>
   <depend>hardware_interface</depend>
   <depend>geometry_msgs</depend>
-  <depend>jackal_msgs</depend>
+  <depend>clearpath_platform_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>

src/jackal_hardware.cpp

@@ -33,7 +33,7 @@
  */
 
 #include "jackal_hardware/jackal_hardware.hpp"
-#include "jackal_msgs/msg/feedback.hpp"
+#include "clearpath_platform_msgs/msg/feedback.hpp"
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 
 namespace jackal_hardware
@@ -61,7 +61,7 @@ void JackalHardware::writeCommandsToHardware()
   node_->drive_command(
     static_cast<float>(diff_speed_left),
     static_cast<float>(diff_speed_right),
-    jackal_msgs::msg::Drive::MODE_VELOCITY);
+    clearpath_platform_msgs::msg::Drive::MODE_VELOCITY);
 }
 
 /**
@@ -72,19 +72,19 @@ void JackalHardware::writeCommandsToHardware()
 void JackalHardware::updateJointsFromHardware()
 {
   rclcpp::spin_some(node_);
-  jackal_msgs::msg::Feedback msg = node_->get_feedback();
+  clearpath_platform_msgs::msg::Feedback msg = node_->get_feedback();
   RCLCPP_DEBUG(
     rclcpp::get_logger(HW_NAME),
     "Received linear distance information (L: %f, R: %f)",
     msg.drivers[0].measured_travel, msg.drivers[1].measured_travel);
 
-  auto side = jackal_msgs::msg::Drive::LEFT;
+  auto side = clearpath_platform_msgs::msg::Drive::LEFT;
   for (auto i = 0u; i < hw_states_position_.size(); i++) {
     if (i == wheel_joints_[RIGHT_ALT_JOINT_NAME] || i == wheel_joints_[RIGHT_CMD_JOINT_NAME]){
-      side = jackal_msgs::msg::Drive::RIGHT;
+      side = clearpath_platform_msgs::msg::Drive::RIGHT;
     }
     else {
-      side = jackal_msgs::msg::Drive::LEFT;
+      side = clearpath_platform_msgs::msg::Drive::LEFT;
     }
 
     double delta = msg.drivers[side].measured_travel -
@@ -105,16 +105,17 @@ void JackalHardware::updateJointsFromHardware()
   }
 }
 
-hardware_interface::return_type JackalHardware::configure(
+hardware_interface::CallbackReturn JackalHardware::on_init(
   const hardware_interface::HardwareInfo & info)
 {
-  if (configure_default(info) != hardware_interface::return_type::OK) {
-    return hardware_interface::return_type::ERROR;
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
+  {
+    return CallbackReturn::ERROR;
   }
 
   RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Name: %s", info_.name.c_str());
 
-  RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Number of Joints %u", info_.joints.size());
+  RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Number of Joints %lu", info_.joints.size());
 
   hw_states_position_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
   hw_states_position_offset_.resize(info_.joints.size(), std::numeric_limits<double>::quiet_NaN());
@@ -128,47 +129,45 @@ hardware_interface::return_type JackalHardware::configure(
     if (joint.command_interfaces.size() != 1) {
       RCLCPP_FATAL(
         rclcpp::get_logger(HW_NAME),
-        "Joint '%s' has %d command interfaces found. 1 expected.", joint.name.c_str(),
+        "Joint '%s' has %ld command interfaces found. 1 expected.", joint.name.c_str(),
         joint.command_interfaces.size());
-      return hardware_interface::return_type::ERROR;
+      return CallbackReturn::ERROR;
     }
 
     if (joint.command_interfaces[0].name != hardware_interface::HW_IF_VELOCITY) {
       RCLCPP_FATAL(
         rclcpp::get_logger(HW_NAME),
         "Joint '%s' have %s command interfaces found. '%s' expected.", joint.name.c_str(),
         joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_VELOCITY);
-      return hardware_interface::return_type::ERROR;
+      return CallbackReturn::ERROR;
     }
 
     if (joint.state_interfaces.size() != 2) {
       RCLCPP_FATAL(
         rclcpp::get_logger(HW_NAME),
-        "Joint '%s' has %d state interface. 2 expected.", joint.name.c_str(),
-        joint.state_interfaces.size());
-      return hardware_interface::return_type::ERROR;
+        "Joint '%s' has %ld state interface. 2 expected.", joint.name.c_str(),
+          joint.state_interfaces.size());
+      return CallbackReturn::ERROR;
     }
 
     if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
       RCLCPP_FATAL(
         rclcpp::get_logger(HW_NAME),
-        "Joint '%s' have '%s' as first state interface. '%s' and '%s' expected.",
-        joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
-        hardware_interface::HW_IF_POSITION);
-      return hardware_interface::return_type::ERROR;
+        "Joint '%s' have '%s' as first state interface. ",
+          joint.name.c_str(), joint.state_interfaces[0].name.c_str());
+      return CallbackReturn::ERROR;
     }
 
     if (joint.state_interfaces[1].name != hardware_interface::HW_IF_VELOCITY) {
       RCLCPP_FATAL(
         rclcpp::get_logger(HW_NAME),
-        "Joint '%s' have '%s' as second state interface. '%s' expected.", joint.name.c_str(),
-        joint.state_interfaces[1].name.c_str(), hardware_interface::HW_IF_VELOCITY);
-      return hardware_interface::return_type::ERROR;
+        "Joint '%s' have '%s' as second state interface. .", joint.name.c_str(),
+        joint.state_interfaces[1].name.c_str());
+      return CallbackReturn::ERROR;
     }
   }
 
-  status_ = hardware_interface::status::CONFIGURED;
-  return hardware_interface::return_type::OK;
+  return CallbackReturn::SUCCESS;
 }
 
 std::vector<hardware_interface::StateInterface> JackalHardware::export_state_interfaces()
@@ -202,7 +201,7 @@ std::vector<hardware_interface::CommandInterface> JackalHardware::export_command
   return command_interfaces;
 }
 
-hardware_interface::return_type JackalHardware::start()
+hardware_interface::CallbackReturn JackalHardware::on_activate(const rclcpp_lifecycle::State & )
 {
   RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Starting ...please wait...");
 
@@ -216,25 +215,21 @@ hardware_interface::return_type JackalHardware::start()
     }
   }
 
-  status_ = hardware_interface::status::STARTED;
-
   RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "System Successfully started!");
 
-  return hardware_interface::return_type::OK;
+  return CallbackReturn::SUCCESS;
 }
 
-hardware_interface::return_type JackalHardware::stop()
+hardware_interface::CallbackReturn JackalHardware::on_deactivate(const rclcpp_lifecycle::State & )
 {
   RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "Stopping ...please wait...");
 
-  status_ = hardware_interface::status::STOPPED;
-
   RCLCPP_INFO(rclcpp::get_logger(HW_NAME), "System successfully stopped!");
 
-  return hardware_interface::return_type::OK;
+  return CallbackReturn::SUCCESS;
 }
 
-hardware_interface::return_type JackalHardware::read()
+hardware_interface::return_type JackalHardware::read(const rclcpp::Time & , const rclcpp::Duration & )
 {
   RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Reading from hardware");
 
@@ -245,7 +240,7 @@ hardware_interface::return_type JackalHardware::read()
   return hardware_interface::return_type::OK;
 }
 
-hardware_interface::return_type JackalHardware::write()
+hardware_interface::return_type JackalHardware::write(const rclcpp::Time & , const rclcpp::Duration & )
 {
   RCLCPP_DEBUG(rclcpp::get_logger(HW_NAME), "Writing to hardware");