Dashing update (#7)

* use dashing rclcpp logger interface

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* avoid deprecated publisher and subscribe API

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* use dashing parameter API

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* Update build to dashing

Signed-off-by: Anas Abou Allaban <allabana@amazon.com>

* apply to master API

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

* address windows warnings

Signed-off-by: Karsten Knese <karsten@openrobotics.org>

Co-authored-by: Anas Abou Allaban <aabouallaban@protonmail.com>

.travis.yml

@@ -7,11 +7,11 @@ install:
 
 matrix:
   include:
-    - env: JOB_TYPE=crystal
-      script: .ros2ci/travis.bash $JOB_TYPE
-    # Uncomment the following to test against ROS 2 Dashing
-    # - env: JOB_TYPE=dashing
+    # - env: JOB_TYPE=crystal
     #   script: .ros2ci/travis.bash $JOB_TYPE
+    # Uncomment the following to test against ROS 2 Dashing
+     - env: JOB_TYPE=dashing
+       script: .ros2ci/travis.bash $JOB_TYPE
     # Uncomment the following to test against ROS 2 nightly build
     # - env: JOB_TYPE=nightly
     #   script: .ros2ci/travis.bash $JOB_TYPE

ros_controllers/include/ros_controllers/joint_state_controller.hpp

@@ -50,7 +50,7 @@ class JointStateController : public controller_interface::ControllerInterface
   std::vector<const hardware_interface::JointStateHandle *> registered_joint_handles_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<sensor_msgs::msg::JointState>>
   joint_state_publisher_;
-  std::shared_ptr<sensor_msgs::msg::JointState> joint_state_msg_;
+  sensor_msgs::msg::JointState joint_state_msg_;
 };
 
 }  // namespace ros_controllers

ros_controllers/include/ros_controllers/joint_trajectory_controller.hpp

@@ -46,6 +46,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     const std::vector<std::string> & joint_names,
     const std::vector<std::string> & write_op_names);
 
+  ROS_CONTROLLERS_PUBLIC
+  controller_interface::controller_interface_ret_t
+  init(
+    std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
+    const std::string & controller_name) override;
+
+  ROS_CONTROLLERS_PUBLIC
+  controller_interface::controller_interface_ret_t
+  update() override;
+
   ROS_CONTROLLERS_PUBLIC
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_configure(const rclcpp_lifecycle::State & previous_state) override;
@@ -70,10 +80,6 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
   on_shutdown(const rclcpp_lifecycle::State & previous_state) override;
 
-  ROS_CONTROLLERS_PUBLIC
-  controller_interface::controller_interface_ret_t
-  update() override;
-
 private:
   std::vector<std::string> joint_names_;
   std::vector<std::string> write_op_names_;

ros_controllers/src/joint_state_controller.cpp

@@ -45,20 +45,19 @@ JointStateController::on_configure(const rclcpp_lifecycle::State & previous_stat
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR;
   }
 
-  joint_state_msg_ = std::make_shared<sensor_msgs::msg::JointState>();
   size_t num_joints = registered_joint_handles_.size();
   // default initialize joint state message
-  joint_state_msg_->position.resize(num_joints);
-  joint_state_msg_->velocity.resize(num_joints);
-  joint_state_msg_->effort.resize(num_joints);
+  joint_state_msg_.position.resize(num_joints);
+  joint_state_msg_.velocity.resize(num_joints);
+  joint_state_msg_.effort.resize(num_joints);
   // set known joint names
-  joint_state_msg_->name.reserve(num_joints);
+  joint_state_msg_.name.reserve(num_joints);
   for (auto joint_handle : registered_joint_handles_) {
-    joint_state_msg_->name.push_back(joint_handle->get_name());
+    joint_state_msg_.name.push_back(joint_handle->get_name());
   }
 
-  joint_state_publisher_ =
-    lifecycle_node_->create_publisher<sensor_msgs::msg::JointState>("joint_states");
+  joint_state_publisher_ = lifecycle_node_->create_publisher<sensor_msgs::msg::JointState>(
+    "joint_states", rclcpp::SystemDefaultsQoS());
   joint_state_publisher_->on_activate();
 
   return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -72,12 +71,12 @@ JointStateController::update()
     return hardware_interface::HW_RET_ERROR;
   }
 
-  joint_state_msg_->header.stamp = rclcpp::Clock().now();
+  joint_state_msg_.header.stamp = rclcpp::Clock().now();
   size_t i = 0;
   for (auto joint_state_handle : registered_joint_handles_) {
-    joint_state_msg_->position[i] = joint_state_handle->get_position();
-    joint_state_msg_->velocity[i] = joint_state_handle->get_velocity();
-    joint_state_msg_->effort[i] = joint_state_handle->get_effort();
+    joint_state_msg_.position[i] = joint_state_handle->get_position();
+    joint_state_msg_.velocity[i] = joint_state_handle->get_velocity();
+    joint_state_msg_.effort[i] = joint_state_handle->get_effort();
     ++i;
   }