Dashing updates (ros-controls#19)

* use dashing rclcpp logger interface

Signed-off-by: Karsten Knese <[email protected]>

* use dashing parameter API

Signed-off-by: Karsten Knese <[email protected]>

* fixed typo on nullptr check (ros-controls#20)

Fixed a nullptr check typo causing exceptions to be thrown whenever
input command is set to 0.
Signed-off-by: Poh Zhi-Ee <[email protected]>

* remove if/else

Signed-off-by: Yutaka Kondo <[email protected]>

* update travis JOB_TYPE

Signed-off-by: Yutaka Kondo <[email protected]>

* Update joint_command_handle.cpp

Co-authored-by: Poh Zhi-Ee <[email protected]>
Co-authored-by: Yutaka Kondo <[email protected]>

Author: 3 people

.travis.yml

@@ -6,11 +6,8 @@ install:
 
 matrix:
   include:
-    - env: JOB_TYPE=crystal
+    - env: JOB_TYPE=dashing
       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

controller_interface/include/controller_interface/controller_interface.hpp

@@ -43,28 +43,24 @@ class ControllerInterface : public rclcpp_lifecycle::node_interfaces::LifecycleN
   ~ControllerInterface() = default;
 
   CONTROLLER_INTERFACE_PUBLIC
-  controller_interface_ret_t
   virtual
+  controller_interface_ret_t
   init(
     std::weak_ptr<hardware_interface::RobotHardware> robot_hardware,
     const std::string & controller_name);
 
-  CONTROLLER_INTERFACE_PUBLIC
-  std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
-  get_lifecycle_node();
-
   CONTROLLER_INTERFACE_PUBLIC
   virtual
   controller_interface_ret_t
   update() = 0;
 
+  CONTROLLER_INTERFACE_PUBLIC
+  std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
+  get_lifecycle_node();
+
 protected:
   std::weak_ptr<hardware_interface::RobotHardware> robot_hardware_;
   std::shared_ptr<rclcpp_lifecycle::LifecycleNode> lifecycle_node_;
-  std::shared_ptr<rclcpp::AsyncParametersClient> parameters_client_;
-
-private:
-  std::shared_ptr<rclcpp::Node> parameter_client_node_;
 };
 
 }  // namespace controller_interface

controller_interface/src/controller_interface.cpp

@@ -46,14 +46,6 @@ ControllerInterface::init(
   lifecycle_node_->register_on_error(
     std::bind(&ControllerInterface::on_error, this, std::placeholders::_1));
 
-  std::string remote_controller_parameter_server = "controller_parameter_server";
-  parameters_client_ = std::make_shared<rclcpp::AsyncParametersClient>(
-    lifecycle_node_->get_node_base_interface(),
-    lifecycle_node_->get_node_topics_interface(),
-    lifecycle_node_->get_node_graph_interface(),
-    lifecycle_node_->get_node_services_interface(),
-    remote_controller_parameter_server);
-
   return CONTROLLER_INTERFACE_RET_SUCCESS;
 }
 

controller_manager/src/controller_manager.cpp

@@ -67,40 +67,36 @@ namespace
 std::string
 parse_library_path(
   const std::string & package_name,
-  const std::string & class_name)
+  const std::string & class_name,
+  const rclcpp::Logger logger)  // should be const ref when possible
 {
-  std::string library_path = "";
-
-  auto allocator = rcutils_get_default_allocator();
   // get node plugin resource from package
   std::string content;
   std::string base_path;
   if (!ament_index_cpp::get_resource(resource_index, package_name, content, &base_path)) {
-    auto error_msg = rcutils_format_string(
-      allocator, "unable to load resource for package %s", package_name.c_str());
-    RCUTILS_LOG_ERROR(error_msg);
+    auto error_msg = std::string("unable to load resource for package ") + package_name;
+    RCLCPP_ERROR(logger, error_msg);
     throw std::runtime_error(error_msg);
   }
 
+  auto allocator = rcutils_get_default_allocator();
   rcutils_string_array_t controller_array = rcutils_get_zero_initialized_string_array();
   rcutils_split(content.c_str(), '\n', allocator, &controller_array);
   if (controller_array.size == 0) {
-    auto error_msg = rcutils_format_string(
-      allocator, "no ros controllers found in package %s", package_name.c_str());
-    RCUTILS_LOG_ERROR(error_msg);
+    auto error_msg = std::string("no ros controllers found in package " + package_name);
+    RCLCPP_ERROR(logger, error_msg);
     throw std::runtime_error(error_msg);
   }
 
+  std::string library_path = "";
   bool controller_is_available = false;
   for (auto i = 0u; i < controller_array.size; ++i) {
     rcutils_string_array_t controller_details = rcutils_get_zero_initialized_string_array();
     rcutils_split(controller_array.data[i], ';', allocator, &controller_details);
     if (controller_details.size != 2) {
-      auto error_msg = rcutils_format_string(
-        allocator,
-        "package resource content has wrong format. should be <class_name>;<library_path>",
-        package_name.c_str());
-      RCUTILS_LOG_ERROR(error_msg);
+      auto error_msg = std::string("package resource content has wrong format") +
+        " - should be <class_name>;<library_path> but is " + controller_array.data[i];
+      RCLCPP_ERROR(logger, error_msg);
       throw std::runtime_error(error_msg);
     }
 
@@ -115,9 +111,8 @@ parse_library_path(
   }
 
   if (!controller_is_available) {
-    auto error_msg = rcutils_format_string(
-      allocator, "couldn't find controller class %s", class_name.c_str());
-    RCUTILS_LOG_ERROR(error_msg);
+    auto error_msg = std::string("couldn't find controller class ") + class_name;
+    RCLCPP_ERROR(logger, error_msg);
     throw std::runtime_error(error_msg);
   }
 
@@ -141,9 +136,9 @@ ControllerManager::load_controller(
   const std::string & class_name,
   const std::string & controller_name)
 {
-  auto library_path = parse_library_path(package_name, class_name);
+  auto library_path = parse_library_path(package_name, class_name, this->get_logger());
 
-  RCUTILS_LOG_INFO("going to load controller %s from library %s\n",
+  RCLCPP_INFO(this->get_logger(), "going to load controller %s from library %s",
     class_name.c_str(), library_path.c_str());
 
   // let possible exceptions escalate

controller_parameter_server/include/controller_parameter_server/parameter_server.hpp

@@ -29,7 +29,11 @@ class ParameterServer : public rclcpp::Node
 {
 public:
   CONTROLLER_PARAMETER_SERVER_PUBLIC
-  ParameterServer();
+  explicit ParameterServer(
+    const rclcpp::NodeOptions & options = (
+      rclcpp::NodeOptions()
+      .allow_undeclared_parameters(true)
+      .automatically_declare_parameters_from_overrides(true)));
 
   CONTROLLER_PARAMETER_SERVER_PUBLIC
   virtual

controller_parameter_server/src/parameter_server.cpp

@@ -21,8 +21,8 @@
 namespace controller_parameter_server
 {
 
-ParameterServer::ParameterServer()
-: rclcpp::Node("controller_parameter_server")
+ParameterServer::ParameterServer(const rclcpp::NodeOptions & options)
+: rclcpp::Node("controller_parameter_server", options)
 {}
 
 void

hardware_interface/src/robot_hardware.cpp

@@ -30,12 +30,9 @@ RobotHardware::register_joint_state_handle(const JointStateHandle * joint_handle
   auto handle_pos = std::find_if(
     registered_joint_state_handles_.begin(), registered_joint_state_handles_.end(),
     [&](auto joint_handle_ptr) -> bool {
-      if (joint_handle_ptr->get_name() == joint_handle->get_name()) {
-        return true;
-      }
-      return false;
-    }
-  );
+      return joint_handle_ptr->get_name() == joint_handle->get_name();
+    });
+
   // handle exist already
   if (handle_pos != registered_joint_state_handles_.end()) {
     return HW_RET_ERROR;