ros2_control simplification and add arm_activate arg (#157)

* Unify configs

* Reduce number of controller config files

* imu_broadcaster/imu -> imu/data

* Update vcs

* Fix build

* Default inactive arm controllers

* Add activate_arm argument

* Fix typo in readme

* Fix arm_activate arg

* Add arm_control script

* Fix pre-commit

* Fix rosbot_hardware_interfaces version

* Fix description

Author: rafal-gorecki

MANIPULATOR.md

@@ -6,11 +6,22 @@
 > 1. **Before starting driver!** Make sure the manipulator is **undock**, manipulator is **away from a collision** (does not rest on robot objects) and **joints are away from its position limits** (e.g. one of the joints is started from extreme position).
 > 2. Controlling MoveIt and via the joystick are two independent processes. You should not send commands to both of these services at the same time.
 > 3. When the power supply is lost, the robot loses momentum and falls by inertia. Therefore, you should hold the manipulator when the power is cut off, or call the docking node `ros2 run open_manipulator_x_moveit dock` or press `RT` + `Back` buttons on gamepad.
-> 4. To improve the range of the manipulator's movements. The robot's URDF has hardcoded that the antenna is set at 90 degrees. It is a good idea to position the antenna in the same way on the physical robot.
+> 4. The manipulator does not analyze collisions with the antenna, to improve the range of the manipulator's movements. It's good practice to position the antenna horizontally on the physical robot.
 > 5. In the event of overload, loss of communication or sudden stopping of the manipulator process (e.g. during reboot), some joints may not receive the command to stop operation. This may prevent re-establishing communication. In such a case, it will be necessary to **reset the power supply**.
 
 Below is a handful of the most important information for the ROSbot Manipulation/Manipulation PRO package.
 
+## Activation
+
+By default, the arm remains in an idle state after the driver is launched. To activate it, execute:
+
+```bash
+sudo rosbot.arm-activate # if you are using snap
+ros2 run rosbot_controller arm_control active # if you are using local build
+```
+
+You can change the driver's default behavior using the `arm_activate` argument.
+
 ## Control
 
 After starting, the manipulator should be in the Home position after a few seconds. Now you can control ROSbot XL and OpenMANIPULATOR-X using a gamepad or RViz.
@@ -56,13 +67,15 @@ To do it, first you will have to disable the torque of the manipulator, for exam
 On your ROSbot XL execute. **Hold the manipulator** while doing it, as it disables the torque and the manipulator can fall.
 
 ```bash
-sudo rosbot.arm-disactivate
+sudo rosbot.arm-disactivate # if you are using snap
+ros2 run rosbot_controller arm_control inactive # if you are using local build
 ```
 
 Now you can manually move the manipulator to the desired position and launch:
 
 ```bash
-sudo rosbot.arm-activate
+sudo rosbot.arm-activate # if you are using snap
+ros2 run rosbot_controller arm_control active # if you are using local build
 ```
 
 ### Modifications

README.md

@@ -99,13 +99,14 @@ ros2 launch rosbot_gazebo simulation.launch.py robot_model:=<rosbot/rosbot_xl>
 
 | 🤖  | 🖥️  | Argument            | Description <br/> **_Type:_** `Default`                                                                                                                                                            |
 | --- | --- | ------------------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| ✅  | ✅  | `arm_activate`    | Whether to activate the manipulator arm on startup.. <br/> **_bool:_** `False` (`True` for simulation)                                                                          |
 | ✅  | ✅  | `config_dir`    | Path to the common configuration directory. You can create such common configuration directory with `ros2 run rosbot_utils create_config_dir {directory}`. <br/> **_string:_** `""`                                                                          |
-| ✅  | ✅  | `configuration` | Specify configuration packages. Currently only ROSbot XL has available packages. Packages: `basic`, `telepresence`, `autonomy`, `manipulation`, `manipulation_pro`, `custom`. <br/> **_string:_** 'basic'                                                                          |
+| ✅  | ✅  | `configuration` | Specify configuration packages. Currently only ROSbot XL has available packages. Packages: `basic`, `telepresence`, `autonomy`, `manipulation`, `manipulation_pro`, `custom`. <br/> **_string:_** `basic`                                                                          |
 | ✅  | ✅  | `joy_vel`       | The topic name to which velocity commands will be published. <br/> **_string:_** `cmd_vel` |
 | ✅  | ✅  | `mecanum`           | Whether to use mecanum drive controller, otherwise use diff drive. <br/> **_bool:_** `False`                                                                                       |
 | ✅  | ✅  | `namespace`         | Add namespace to all launched nodes. <br/> **_string:_** `env(ROBOT_NAMESPACE)`                                                                                                                       |
 | ✅  | ✅  | `robot_model`       | Specify robot model. <br/> **_string:_** `env(ROBOT_MODEL_NAME)` (choices: `rosbot`, `rosbot_xl`)                                                                                                                       |
-| ✅  | ❌  | `manipulator_serial_port`  | Port to connect to the manipulator. <br/> **_string:_** `8888`                                                                                                                                  |
+| ✅  | ❌  | `manipulator_serial_port`  | Port to connect to the manipulator. <br/> **_string:_** `/dev/ttyUSB0`                                                                                                                                  |
 | ✅  | ❌  | `microros`          | Automatically connect with hardware using microros. <br/> **_bool:_** `True`                                                                                                                       |
 | ✅  | ❌  | `port`              | **ROSbot XL only.** UDP4 port for micro-ROS agent. <br/> **_string:_** `8888`                                                                                                                         |
 | ✅  | ❌  | `serial_baudrate`   | ROSbot only. Baud rate for serial communication. <br/> **_string:_** `576000`                                                                                                                                  |

ROS_API.md

@@ -56,7 +56,7 @@ Detailed information about content of rosbot package for ROS2.
 | ✅  | ✅  | **`cmd_vel`**                                  | Sends velocity commands for controlling robot motion. <br /> _[geometry_msgs/Twist]_                                          |
 | ✅  | ✅  | **`diagnostics`**                              | Contains diagnostic information about the robot's systems. <br /> _[diagnostic_msgs/DiagnosticArray]_                         |
 | ✅  | ✅  | **`dynamic_joint_states`**                     | Publishes information about the dynamic state of joints. <br /> _[control_msgs/DynamicJointState]_                            |
-| ✅  | ✅  | **`imu_broadcaster/imu`**                      | Broadcasts IMU (Inertial Measurement Unit) data. <br /> _[sensor_msgs/Imu]_                                                   |
+| ✅  | ✅  | **`imu/data`**                      | Broadcasts IMU (Inertial Measurement Unit) data. <br /> _[sensor_msgs/Imu]_                                                   |
 | ✅  | ✅  | **`joint_states`**                             | Publishes information about the state of robot joints. <br /> _[sensor_msgs/JointState]_                                      |
 | ✅  | ✅  | **`odometry/filtered`**                        | Publishes filtered odometry data. <br /> _[nav_msgs/Odometry]_                                                                |
 | ✅  | ✅  | **`odometry/wheels`**              | Provides odometry data from the base controller of the ROSbot XL. <br /> _[nav_msgs/Odometry]_                                |

rosbot_bringup/test/test_utils.py

@@ -58,7 +58,7 @@ def create_test_subscribers_and_publishers(self):
         self.controller_odom_sub = self.create_subscription(
             Odometry, "odometry/wheels", self.controller_odometry_callback, 10
         )
-        self.imu_sub = self.create_subscription(Imu, "imu_broadcaster/imu", self.imu_callback, 10)
+        self.imu_sub = self.create_subscription(Imu, "imu/data", self.imu_callback, 10)
         self.ekf_odom_sub = self.create_subscription(
             Odometry, "odometry/filtered", self.ekf_odometry_callback, 10
         )

rosbot_controller/config/rosbot/controllers.yaml

@@ -0,0 +1,137 @@
+---
+/**:
+  controller_manager:
+    ros__parameters:
+      update_rate: 100 # Hz
+
+      differential_drive_controller:
+        type: diff_drive_controller/DiffDriveController
+      mecanum_drive_controller:
+        type: mecanum_drive_controller/MecanumDriveController
+      imu_broadcaster:
+        type: imu_sensor_broadcaster/IMUSensorBroadcaster
+      joint_state_broadcaster:
+        type: joint_state_broadcaster/JointStateBroadcaster
+
+  imu_broadcaster:
+    ros__parameters:
+      tf_frame_prefix_enable: false
+      sensor_name: <namespace>/imu
+      frame_id: imu_link
+      static_covariance_orientation: [1.9e-3, 0.0, 0.0, 0.0, 1.9e-3, 0.0, 0.0, 0.0, 1.9e-3] # Values taken from datasheet
+      static_covariance_angular_velocity: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0e-3] # Values measured experimentally
+      static_covariance_linear_acceleration: [6.4e-2, 0.0, 0.0, 0.0, 2.2e-2, 0.0, 0.0, 0.0, 0.0] # Values measured experimentally
+
+  differential_drive_controller:
+    ros__parameters:
+      # Robot Specific
+      tf_frame_prefix_enable: false
+      left_wheel_names: [fl_wheel_joint, rl_wheel_joint]
+      right_wheel_names: [fr_wheel_joint, rr_wheel_joint]
+      wheel_separation: 0.186
+      wheel_radius: 0.0425
+      wheel_separation_multiplier: 1.36 # ICR coefficient isn't totally accurate and this coefficient can differ for various ground types
+      left_wheel_radius_multiplier: 0.95  # Based on real measurements
+      right_wheel_radius_multiplier: 0.95 # Based on real measurements
+      odom_frame_id: odom
+      base_frame_id: base_link
+      pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+      twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally
+
+      # Odometry
+      publish_rate: 20.0
+      open_loop: false
+      position_feedback: false
+      velocity_rolling_window_size: 1
+      enable_odom_tf: false
+
+      # Publication
+      cmd_vel_timeout: 0.5
+      publish_limited_velocity: false
+      use_stamped_vel: true
+
+      # Limits
+      linear:
+        x:
+          max_velocity: 1.0 # m/s
+          min_velocity: -1.0 # m/s
+          max_acceleration: 1.0 # m/s^2
+          max_deceleration: -1.0 # m/s^2
+          max_acceleration_reverse: -1.0 # m/s^2
+          max_deceleration_reverse: 1.0 # m/s^2
+          max_jerk: .nan # m/s^3
+          min_jerk: .nan # m/s^3
+
+      angular:
+        z:
+          max_velocity: 3.14 # rad/s
+          min_velocity: -3.14 # rad/s
+          max_acceleration: 4.0 # rad/s^2
+          max_deceleration: -4.0 # rad/s^2
+          max_acceleration_reverse: -4.0 # rad/s^2
+          max_deceleration_reverse: 4.0 # rad/s^2
+          max_jerk: .nan # rad/s^3
+          min_jerk: .nan # rad/s^3
+
+  mecanum_drive_controller:
+    ros__parameters:
+      # Robot Specific
+      tf_frame_prefix_enable: false
+      front_left_wheel_name: fl_wheel_joint
+      front_right_wheel_name: fr_wheel_joint
+      rear_left_wheel_name: rl_wheel_joint
+      rear_right_wheel_name: rr_wheel_joint
+      wheel_separation_x: 0.106
+      wheel_separation_y: 0.192
+      wheel_radius: 0.047
+      wheel_separation_x_multiplier: 1.0
+      wheel_separation_y_multiplier: 1.0
+      wheel_radius_multiplier: 1.0
+      odom_frame_id: odom
+      base_frame_id: base_link
+      pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+      twist_covariance_diagonal: [3.3e-5, 3.3e-5, 0.0, 0.0, 0.0, 2.8e-3] # Values measured experimentally
+
+      # Odometry
+      publish_rate: 25.0
+      open_loop: false
+      position_feedback: false
+      velocity_rolling_window_size: 1
+      enable_odom_tf: false
+
+      # Publication
+      cmd_vel_timeout: 0.5
+      publish_limited_velocity: false
+      use_stamped_vel: true
+
+      # Limits
+      linear:
+        x:
+          max_velocity: 1.0 # m/s
+          min_velocity: -1.0 # m/s
+          max_acceleration: 0.75 # m/s^2
+          max_deceleration: -0.75 # m/s^2
+          max_acceleration_reverse: -0.75 # m/s^2
+          max_deceleration_reverse: 0.75 # m/s^2
+          max_jerk: .nan # m/s^3
+          min_jerk: .nan # m/s^3
+        y:
+          max_velocity: 1.0 # m/s
+          min_velocity: -1.0 # m/s
+          max_acceleration: 0.75 # m/s^2
+          max_deceleration: -0.75 # m/s^2
+          max_acceleration_reverse: -0.75 # m/s^2
+          max_deceleration_reverse: 0.75 # m/s^2
+          max_jerk: .nan # m/s^3
+          min_jerk: .nan # m/s^3
+
+      angular:
+        z:
+          max_velocity: 3.14 # rad/s
+          min_velocity: -3.14 # rad/s
+          max_acceleration: 3.0 # rad/s^2
+          max_deceleration: -3.0 # rad/s^2
+          max_acceleration_reverse: -3.0 # rad/s^2
+          max_deceleration_reverse: 3.0 # rad/s^2
+          max_jerk: .nan # rad/s^3
+          min_jerk: .nan # rad/s^3