Update control error log (#24)

* Use RCLCPP_ERROR instead of throwing runtime error * Add emulate_tty option to show colorized error logs
rt-net · Nov 10, 2020 · 8e9c9dc · 8e9c9dc
1 parent a73b352
commit 8e9c9dc
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 4 deletions.
diff --git a/crane_plus_control/launch/crane_plus_control.launch.py b/crane_plus_control/launch/crane_plus_control.launch.py
@@ -31,6 +31,7 @@ def generate_launch_description():
         package='crane_plus_control',
         executable='crane_plus_control_node',
         output='screen',
+        emulate_tty=True,  # This results error logs are colorized in a terminal.
         parameters=[{'controller_name': 'crane_plus_arm_controller'},
                     os.path.join(get_package_share_directory('crane_plus_control'),
                                  'config', 'crane_plus_controllers.yaml'),

diff --git a/crane_plus_control/src/crane_plus_interface.cpp b/crane_plus_control/src/crane_plus_interface.cpp
@@ -43,7 +43,8 @@ hardware_interface::return_type CranePlusInterface::init(
   driver_ = std::make_shared<CranePlusDriver>(port_name, baudrate, dxl_id_list);
 
   if (!driver_->open_port()) {
-    throw std::runtime_error(driver_->get_last_error_log());
+    RCLCPP_ERROR(LOGGER, driver_->get_last_error_log());
+    return hardware_interface::return_type::ERROR;
   }
 
   for (auto joint_name : joint_name_list) {
@@ -68,22 +69,25 @@ hardware_interface::return_type CranePlusInterface::init(
     if (register_joint_state_handle(&joint_state_handles_[i]) !=
       hardware_interface::return_type::OK)
     {
-      throw std::runtime_error("unable to register " + joint_state_handles_[i].get_name());
+      RCLCPP_ERROR(LOGGER, "Unable to register %s.", joint_state_handles_[i].get_name());
+      return hardware_interface::return_type::ERROR;
     }
 
     hardware_interface::JointCommandHandle command_handle(joint_name, &cmd_[i]);
     joint_command_handles_[i] = command_handle;
     if (register_joint_command_handle(&joint_command_handles_[i]) !=
       hardware_interface::return_type::OK)
     {
-      throw std::runtime_error("unable to register " + joint_command_handles_[i].get_name());
+      RCLCPP_ERROR(LOGGER, "Unable to register %s.", joint_command_handles_[i].get_name());
+      return hardware_interface::return_type::ERROR;
     }
 
     joint_mode_handles_[i] = hardware_interface::OperationModeHandle(joint_name, &op_mode_[i]);
     if (register_operation_mode_handle(&joint_mode_handles_[i]) !=
       hardware_interface::return_type::OK)
     {
-      throw std::runtime_error("unable to register " + joint_mode_handles_[i].get_name());
+      RCLCPP_ERROR(LOGGER, "Unable to register %s.", joint_mode_handles_[i].get_name());
+      return hardware_interface::return_type::ERROR;
     }
     ++i;
   }