Merge branch 'serial_communication_fixes' into serial_communication-devel

Author: slimasziom

robotiq_hande_driver/hardware/include/robotiq_hande_driver/application.hpp

@@ -52,7 +52,7 @@ class GripperApplication {
      * @return None.
      * @note The status should be checked to verify successful execution. An exception is thrown if communication issues occur.
      */
-    void initialize(double gripper_position_min, double gripper_position_max, std::string tty_port, int baudrate, char parity, int data_bits, int stop_bit, int slave_id) {
+    void initialize(double gripper_position_min, double gripper_position_max, std::string& tty_port, int baudrate, char parity, int data_bits, int stop_bit, int slave_id) {
         gripper_position_min_ = gripper_position_min;
         gripper_position_max_ = gripper_position_max;
         gripper_postion_step_ = (gripper_position_max_ - gripper_position_min_) / 255.0;

robotiq_hande_driver/hardware/include/robotiq_hande_driver/communication.hpp

@@ -60,7 +60,8 @@ class Communication{
      * @return None.
      * @note The status should be checked to verify successful execution. An exception is thrown if communication issues occur.
      */
-    void initialize(std::string tty_port, int baudrate, char parity, int data_bits, int stop_bit, int slave_id) {
+    void initialize(std::string& tty_port, int baudrate, char parity, int data_bits, int stop_bit, int slave_id) {
+
         tty_port_ = tty_port.c_str();
         baudrate_ = baudrate;
         parity_ = parity;
@@ -79,6 +80,7 @@ class Communication{
     int configure() {
         int result;
 
+        printf("Connecting to: %s, %d, %c, %d, %d\n", tty_port_, baudrate_, parity_, data_bits_, stop_bit_);
         mb_ = modbus_new_rtu(tty_port_, baudrate_, parity_, data_bits_, stop_bit_);
         modbus_set_slave(mb_, slave_id_);
         modbus_set_debug(mb_, DEBUG_MODBUS);

robotiq_hande_driver/hardware/include/robotiq_hande_driver/protocol_logic.hpp

@@ -106,7 +106,7 @@ class ProtocolLogic{
      * @return None.
      * @note The status should be checked to verify successful execution. An exception is thrown if communication issues occur.
      */
-    void initialize(std::string tty_port, int baudrate, char parity, int data_bits, int stop_bit, int slave_id) {
+    void initialize(std::string& tty_port, int baudrate, char parity, int data_bits, int stop_bit, int slave_id) {
         communication_.initialize(tty_port, baudrate, parity, data_bits, stop_bit, slave_id);
     };
 

robotiq_hande_driver/hardware/src/communication.cpp

@@ -25,6 +25,6 @@ int Communication::connect(){
     else
         printf("Couldn't connect: %d\n", result);
 
-    return result;
+    return 1;
 }
 }   // namespace robotiq_hande_driver

robotiq_hande_driver/hardware/src/hande_hardware_interface.cpp

@@ -11,17 +11,29 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init(const HWI::HardwareIn
     if(HWI::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
         return HWI::CallbackReturn::ERROR;
     }
+    logger_ = std::make_shared<rclcpp::Logger>(
+    rclcpp::get_logger("controller_manager.resource_manager.hardware_component.system.RobotiqHandeHardwareInterface"));
+
+
+    RCLCPP_DEBUG(get_logger(), "grip_pos_min: %s", info_.hardware_parameters["grip_pos_min"].c_str());
+    RCLCPP_DEBUG(get_logger(), "grip_pos_max: %s", info_.hardware_parameters["grip_pos_max"].c_str());
+    RCLCPP_DEBUG(get_logger(), "tty: %s", info_.hardware_parameters["tty"].c_str());
+    RCLCPP_DEBUG(get_logger(), "baudrate: %s", info_.hardware_parameters["baudrate"].c_str());
+    RCLCPP_DEBUG(get_logger(), "parity: %s", info_.hardware_parameters["parity"].c_str());
+    RCLCPP_DEBUG(get_logger(), "data_bits: %s", info_.hardware_parameters["data_bits"].c_str());
+    RCLCPP_DEBUG(get_logger(), "stop_bit:  %s", info_.hardware_parameters["stop_bit"].c_str());
+    RCLCPP_DEBUG(get_logger(), "slave_id: %s", info_.hardware_parameters["slave_id"].c_str());
 
     state_velocity_ = 0.0;
     cmd_force_ = 1.0;
-    gripper_position_min_ = stod(info_.hardware_parameters["grip_pos_min"]);
-    gripper_position_max_ = stod(info_.hardware_parameters["grip_pos_max"]);
+    gripper_position_min_ = std::stod(info_.hardware_parameters["grip_pos_min"]);
+    gripper_position_max_ = std::stod(info_.hardware_parameters["grip_pos_max"]);
     tty_port_ = info_.hardware_parameters["tty"];
-    baudrate_ = stoi(info_.hardware_parameters["baudrate"]);
-    parity_ = info_.hardware_parameters["parity"][0];
-    data_bits_ = stoi(info_.hardware_parameters["data_bits"]);
-    stop_bit_ = stoi(info_.hardware_parameters["stop_bit"]);
-    slave_id_ = stoi(info_.hardware_parameters["slave_id"]);
+    baudrate_ = std::stoi(info_.hardware_parameters["baudrate"]);
+    parity_ = (info_.hardware_parameters["parity"].c_str())[0];
+    data_bits_ = std::stoi(info_.hardware_parameters["data_bits"]);
+    stop_bit_ = std::stoi(info_.hardware_parameters["stop_bit"]);
+    slave_id_ = std::stoi(info_.hardware_parameters["slave_id"]);
 
     cmd_position_ = gripper_position_max_;
     state_position_ = gripper_position_max_;
@@ -32,7 +44,7 @@ HWI::CallbackReturn RobotiqHandeHardwareInterface::on_init(const HWI::HardwareIn
 
     application_layer_.initialize(gripper_position_min_, gripper_position_max_, tty_port_, baudrate_, parity_, data_bits_, stop_bit_, slave_id_);
 
-    RCLCPP_INFO(get_logger(), "Initialized ModbusRTU for %s", tty_port_.c_str());
+    RCLCPP_INFO(get_logger(), "Initialized ModbusRTU for %s, %d, %c, %d, %d", tty_port_.c_str(), baudrate_, parity_, data_bits_, stop_bit_);
     return HWI::CallbackReturn::SUCCESS;
 }