add support for cabot3-kx (#33)

* add odriver can

Signed-off-by: Reiji Nakano <[email protected]>

* create parameters of motor gain

Signed-off-by: Kenta Konagaya <[email protected]>

* [add] Add power_controller_kx

Signed-off-by: kantakozeni0213 <[email protected]>

* add dummy node of set_vel_gains_node

Signed-off-by: Kenta Konagaya <[email protected]>

* [change] Change of Copyright

Signed-off-by: kantakozeni0213 <[email protected]>

* [change] Change service name

Signed-off-by: kantakozeni0213 <[email protected]>

* delete individual gains and combine them

Signed-off-by: Kenta Konagaya <[email protected]>

* modify function name

Signed-off-by: Kenta Konagaya <[email protected]>

* add odriver_can to launch file

Signed-off-by: Kenta Konagaya <[email protected]>

* add copyright info

Signed-off-by: Kenta Konagaya <[email protected]>

* [add] Add CAN communication

Signed-off-by: kantakozeni0213 <[email protected]>

* [refactor] Organize functions that perform CAN communication

Signed-off-by: kozenikanta <[email protected]>

* [rename] Rename directory power_controller/power_controller_kx/power_controller to power_controller/power_controller_kx/power_controller_kx

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor FanController.srv

Signed-off-by: kozenikanta <[email protected]>

* [add] Add License

Signed-off-by: kozenikanta <[email protected]>

* [change] Change license to MIT

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor code for better readability and maintainability

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor code for better readability and maintainability

Signed-off-by: kozenikanta <[email protected]>

* fix format

Signed-off-by: Daisuke Sato <[email protected]>

* adjust unittest

Signed-off-by: Daisuke Sato <[email protected]>

* remove unused rate

Signed-off-by: Daisuke Sato <[email protected]>

* [remove] Remove Set12vPowerPC service

Signed-off-by: kozenikanta <[email protected]>

* [fix] Ensure fan control PWM value does not go negative

Signed-off-by: kozenikanta <[email protected]>

* [change] Change perror to RCLCPP_ERROR

Signed-off-by: kozenikanta <[email protected]>

* [fix] Retrieve data received via CAN communication without using an infinite loop

Signed-off-by: kozenikanta <[email protected]>

* [fix] Fix service communication failure when CAN communication fails

Signed-off-by: kozenikanta <[email protected]>

* [fix] Fixed spelling mistake in the plural of 'battery

Signed-off-by: kozenikanta <[email protected]>

* [change] Changed 'Odrive' to 'odrive'

Signed-off-by: kozenikanta <[email protected]>

* [change] Changed from UpperCamelCase to lowerCamelCase

Signed-off-by: kozenikanta <[email protected]>

* Add temperature measurement node

Signed-off-by: Reiji Nakano <[email protected]>

* [remove] Remove uppercase in service name

Signed-off-by: kozenikanta <[email protected]>

* Changed temperature measurement node

Signed-off-by: Reiji Nakano <[email protected]>

* [update] Make CAN interface configurable via ROS parameter

Signed-off-by: kozenikanta <[email protected]>

* [rename] Rename can_id to id

Signed-off-by: kozenikanta <[email protected]>

* Changed temperature measurement node

Signed-off-by: Reiji Nakano <[email protected]>

* [fix] Fix function argument type declarations

Signed-off-by: kozenikanta <[email protected]>

* [rename] Rename reboot and shutdown functions

Signed-off-by: kozenikanta <[email protected]>

* [fix] Fix count size of memcpy

Signed-off-by: kozenikanta <[email protected]>

* [convert] Convert create_wall_timer period to constant

Signed-off-by: kozenikanta <[email protected]>

* [change] Change fan control from service to topic communication

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor and improve code readability

Signed-off-by: kozenikanta <[email protected]>

* [change] Changes to the Fan Control PWM Specification

Signed-off-by: kozenikanta <[email protected]>

* add the odriver_can_adapter package

Signed-off-by: yushi-minemura <[email protected]>

* update copyright in Dockerfile

Signed-off-by: yushi-minemura <[email protected]>

* [fix] Fix bug where data was being overwritten when multiple services were called, only reflecting the last called service's data

Signed-off-by: kozenikanta <[email protected]>

* [add] Add unit conversion calculations for data received via CAN communication

Signed-off-by: kozenikanta <[email protected]>

* [rename] Rename FRAMOS service name

Signed-off-by: kozenikanta <[email protected]>

* parameterize the diameter variable using ROS parameters

Signed-off-by: yushi-minemura <[email protected]>

* define kVelocityControlMode and kPassthroughInputMode constants

Signed-off-by: yushi-minemura <[email protected]>

* fix compile error

Signed-off-by: yushi-minemura <[email protected]>

* delete unnecessary code

Signed-off-by: yushi-minemura <[email protected]>

* change to publish the motor_status topic

Signed-off-by: yushi-minemura <[email protected]>

* align the code formatting in test.launch.py

Signed-off-by: yushi-minemura <[email protected]>

* Addition of sensor function

Signed-off-by: Reiji Nakano <[email protected]>

* Fix touch sensor  IMU and IMU calibration code

Signed-off-by: Reiji Nakano <[email protected]>

* add parameter to reverse the sign of the velocity command based on the wheel direction

Signed-off-by: yushi-minemura <[email protected]>

* add the is_clockwise parameter

Signed-off-by: yushi-minemura <[email protected]>

* add the hz parameter

Signed-off-by: yushi-minemura <[email protected]>

* update comment about loop_ctrl

Signed-off-by: yushi-minemura <[email protected]>

* WIP: change to call a service to switch the mode based on the loop_ctrl topic

Signed-off-by: yushi-minemura <[email protected]>

* lock the request_axis_state service from being called while it is being processed

Signed-off-by: yushi-minemura <[email protected]>

* format the code for odriver_can_adapter_node.cpp

Signed-off-by: yushi-minemura <[email protected]>

* [refactor] Refactor code to calculate CAN communication data and embed the results in the message

Signed-off-by: kozenikanta <[email protected]>

* add service_timeout_ms parameter

Signed-off-by: yushi-minemura <[email protected]>

* delete WIP comment

Signed-off-by: yushi-minemura <[email protected]>

* Add new feature, Fix bug

Signed-off-by: Reiji Nakano <[email protected]>

* Remove commented-out code

Signed-off-by: Reiji Nakano <[email protected]>

* define AxisStateClientInfo struct type to group the data

Signed-off-by: yushi-minemura <[email protected]>

* consolidate the duplicated processing into a function

Signed-off-by: yushi-minemura <[email protected]>

* change the qos setting from KeepAll to KeepLast(10)

Signed-off-by: yushi-minemura <[email protected]>

* add the ODriveManager class

Signed-off-by: yushi-minemura <[email protected]>

* implement ODriveManager class

Signed-off-by: yushi-minemura <[email protected]>

* replace part of the processing with the ODriveManager class

Signed-off-by: yushi-minemura <[email protected]>

* [refactor] Encapsulate the same process into a function and organize the code

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Standardize the casing of output messages and remove unnecessary code

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor code for better organization

Signed-off-by: kozenikanta <[email protected]>

* Add launch file and configure for quick node restart

Signed-off-by: kozenikanta <[email protected]>

* Add servo motor control and TOF sensor functionality

Signed-off-by: Reiji Nakano <[email protected]>

* Fix code formatting

Signed-off-by: Reiji Nakano <[email protected]>

* Fix code to invert the sign of the data due to negative current reception

Signed-off-by: kozenikanta <[email protected]>

* Modify CMakeLists.txt to remove temp node

Signed-off-by: Reiji Nakano <[email protected]>

* Program changes for the IMU and vibrator sections

Signed-off-by: Reiji Nakano <[email protected]>

* [refactor] Refactor code to comply with naming conventions

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor code to comply with naming conventions

Signed-off-by: kozenikanta <[email protected]>

* [refactor] Refactor code to improve processing speed

Signed-off-by: kozenikanta <[email protected]>

* Fix code for vibrator functionality

Signed-off-by: Reiji Nakano <[email protected]>

* Fix PWM Control Implementation and Fix IMU orientation

Signed-off-by: Reiji Nakano <[email protected]>

* fixed build errors

Signed-off-by: yushi-minemura <[email protected]>

* copied the startup script from the main branch

Signed-off-by: yushi-minemura <[email protected]>

* Change variable names

Signed-off-by: Reiji Nakano <[email protected]>

* clean code

Signed-off-by: Reiji Nakano <[email protected]>

* Modified the layout of pushed components

Signed-off-by: Reiji Nakano <[email protected]>

* clean code

Signed-off-by: Reiji Nakano <[email protected]>

* add option to specify IMU bias

Signed-off-by: Daisuke Sato <[email protected]>

* increase filter crop box size for a work around

Signed-off-by: Daisuke Sato <[email protected]>

* update CAN interface name to match the actual device for k1

Signed-off-by: yushi-minemura <[email protected]>

* change to temporary branch for cabot-description

Signed-off-by: yushi-minemura <[email protected]>

* add cabot_can in volume mount packages for docker

Signed-off-by: yushi-minemura <[email protected]>

* add cabot3-k1 model

Signed-off-by: yushi-minemura <[email protected]>

* remapping pressure topic for cabot3.launch.py

Signed-off-by: yushi-minemura <[email protected]>

* kufusha k3 wip

Signed-off-by: Daisuke Sato <[email protected]>

* adjust max_acc for kx

Signed-off-by: Daisuke Sato <[email protected]>

* add IMU bias adjust for cabot can

Signed-off-by: Daisuke Sato <[email protected]>

* adjust max_acc for k3

Signed-off-by: Daisuke Sato <[email protected]>

* change rslidar frame name

Signed-off-by: Daisuke Sato <[email protected]>

* temporal fix for k3 handle

Signed-off-by: Daisuke Sato <[email protected]>

* Change the range of motion for the servo motor and update variable names

Signed-off-by: Reiji Nakano <[email protected]>

* add ready flag to avoid publishing invalid motor status (#30)

Signed-off-by: Daisuke Sato <[email protected]>

* temporal fix for k3 handle to disable calling setServoFree

Signed-off-by: Masayuki Murata <[email protected]>

* addition of changes related to cabot3-k2

Signed-off-by: yushi-minemura <[email protected]>

* temporary commit fot handle test

Signed-off-by: Yukiya Nakai <[email protected]>

* adjust max_acc for k2

Signed-off-by: Masayuki Murata <[email protected]>

* merged yushim/odriver-can-replace-motor-control

Signed-off-by: yushi-minemura <[email protected]>

* merged yushim/fixed-add-sensor-can

Signed-off-by: yushi-minemura <[email protected]>

* merged yushim/kx-cabot-test

Signed-off-by: yushi-minemura <[email protected]>

* merged yushim/k3-tmp

Signed-off-by: yushi-minemura <[email protected]>

* modify code to coexist of k1 and k3

Signed-off-by: yushi-minemura <[email protected]>

* merged yushim/k2-cabot-test

Signed-off-by: yushi-minemura <[email protected]>

* merged ishihara/kx-cabot-test-enable-livox

Signed-off-by: yushi-minemura <[email protected]>

* add helios.yaml

Signed-off-by: yushi-minemura <[email protected]>

* fix syntax error in dependency.repos

Signed-off-by: yushi-minemura <[email protected]>

* fix syntax error in cabot3.launch.py

Signed-off-by: yushi-minemura <[email protected]>

* add dependency packages for odrive_ros2_control

Signed-off-by: yushi-minemura <[email protected]>

* temporarily change version of dependency packages

Signed-off-by: yushi-minemura <[email protected]>

* fix warnings while building odriver_can_adapter package

Signed-off-by: yushi-minemura <[email protected]>

* change ros_odrive version

Signed-off-by: yushi-minemura <[email protected]>

* merged kenconnor/kx-cabot-test-odrive-test

Co-authored-by: Kenta Konagaya <[email protected]>

Signed-off-by: yushi-minemura <[email protected]>

* add axis_idle_on_shutdown parameter

Signed-off-by: yushi-minemura <[email protected]>

* move ros_odrive dependency from workspace to image

Signed-off-by: Daisuke Sato <[email protected]>

* change package name from power_controller to power_controller_kx / lint power_controller

Signed-off-by: Daisuke Sato <[email protected]>

* update copyright of pacakge.xml

Signed-off-by: Daisuke Sato <[email protected]>

* change copyright

Signed-off-by: Daisuke Sato <[email protected]>

* revert docker/driver/launch.sh

Signed-off-by: Daisuke Sato <[email protected]>

* revert docker-compose.yaml

Signed-off-by: Daisuke Sato <[email protected]>

* revert copyright change

Signed-off-by: Daisuke Sato <[email protected]>

* fix lint error

Signed-off-by: Daisuke Sato <[email protected]>

* revert cabot2-common.yaml

Signed-off-by: Daisuke Sato <[email protected]>

* use model_flags dictionary to determine which components are used for the specified model

Signed-off-by: Daisuke Sato <[email protected]>

* remove odriver_s1_node param from kx

Signed-off-by: Daisuke Sato <[email protected]>

* do not use fixed path

Signed-off-by: Daisuke Sato <[email protected]>

* revert cabot_base/src/cabot/handle_v3.cpp

Signed-off-by: Daisuke Sato <[email protected]>

* remove unused params

Signed-off-by: Daisuke Sato <[email protected]>

* add cabot_can config

Signed-off-by: Daisuke Sato <[email protected]>

* add newline at end of file

Signed-off-by: Daisuke Sato <[email protected]>

---------

Signed-off-by: Reiji Nakano <[email protected]>
Signed-off-by: Kenta Konagaya <[email protected]>
Signed-off-by: kantakozeni0213 <[email protected]>
Signed-off-by: kozenikanta <[email protected]>
Signed-off-by: Daisuke Sato <[email protected]>
Signed-off-by: yushi-minemura <[email protected]>
Signed-off-by: Masayuki Murata <[email protected]>
Signed-off-by: Yukiya Nakai <[email protected]>
Co-authored-by: Reiji Nakano <[email protected]>
Co-authored-by: Kenta Konagaya <[email protected]>
Co-authored-by: kantakozeni0213 <[email protected]>
Co-authored-by: Daisuke Sato <[email protected]>
Co-authored-by: Kanata Kozeni <[email protected]>
Co-authored-by: Daisuke Sato <[email protected]>
Co-authored-by: Masayuki Murata <[email protected]>
Co-authored-by: Yukiya Nakai <[email protected]>

cabot_base/config/cabot3-k1.yaml

@@ -0,0 +1,47 @@
+# config file for cabot3-k1
+
+pointcloud_to_laserscan_node:
+  ros__parameters:
+    angle_min: -2.57  # -M_PI/2 (front) - 1.0 (angle clipping)
+    angle_max: 1.05 # M_PI/2 (rear) - M_PI/6 (rear cover)
+
+# substitute these parameters to hesai/hesai_lidar.yaml
+hesai_lidar:
+  ros__parameters:
+    lidar_type: "PandarXT-32"
+    frame_id: "velodyne" # use velodyne for compatibility
+    timestamp_type: "realtime"
+    lidar_correction_file: "$(find-pkg-share hesai_lidar)/config/PandarXT-32.csv"
+
+cabot:
+  cabot_handle_v2_node:
+    ros__parameters:
+      no_vibration: false
+      buttons:
+        - BUTTON_UP
+        - BUTTON_DOWN
+        - BUTTON_LEFT
+        - BUTTON_RIGHT
+
+  cabot_handle_v3_node:
+    ros__parameters:
+      buttons:
+        - BUTTON_UP
+        - BUTTON_DOWN
+        - BUTTON_LEFT
+        - BUTTON_RIGHT
+
+  cabot_serial:
+    ros__parameters:
+      port: /dev/ttyESP32
+
+  odriver_adapter_node:
+    ros__parameters:
+      bias: 0.14 # same with cabot3-i1
+      gain_omega: 0.0
+      max_acc: 0.6
+
+  cabot_can:
+    ros__parameters:
+      can_interface: can0
+      # calibration_params: [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]

cabot_base/config/cabot3-k2.yaml

@@ -0,0 +1,39 @@
+# config file for cabot3-k2
+
+pointcloud_to_laserscan_node:
+  ros__parameters:
+    angle_min: -2.57  # -M_PI/2 (front) - 1.0 (angle clipping)
+    angle_max: 1.05 # M_PI/2 (rear) - M_PI/6 (rear cover)
+
+cabot:
+  cabot_handle_v2_node:
+    ros__parameters:
+      no_vibration: false
+      buttons:
+        - BUTTON_UP
+        - BUTTON_DOWN
+        - BUTTON_LEFT
+        - BUTTON_RIGHT
+
+  cabot_handle_v3_node:
+    ros__parameters:
+      buttons:
+        - BUTTON_UP
+        - BUTTON_DOWN
+        - BUTTON_LEFT
+        - BUTTON_RIGHT
+
+  cabot_serial:
+    ros__parameters:
+      port: /dev/ttyESP32
+
+  odriver_adapter_node:
+    ros__parameters:
+      bias: 0.14 # same with cabot3-i1
+      gain_omega: 0.0
+      max_acc: 0.6
+
+  cabot_can:
+    ros__parameters:
+      can_interface: can0
+      # calibration_params: [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]

cabot_base/config/cabot3-k3.yaml

@@ -0,0 +1,39 @@
+# config file for cabot3-k3
+
+pointcloud_to_laserscan_node:
+  ros__parameters:
+    angle_min: -2.57  # -M_PI/2 (front) - 1.0 (angle clipping)
+    angle_max: 1.05 # M_PI/2 (rear) - M_PI/6 (rear cover)
+
+cabot:
+  cabot_handle_v2_node:
+    ros__parameters:
+      no_vibration: false
+      buttons:
+        - BUTTON_UP
+        - BUTTON_DOWN
+        - BUTTON_LEFT
+        - BUTTON_RIGHT
+
+  cabot_handle_v3_node:
+    ros__parameters:
+      buttons:
+        - BUTTON_UP
+        - BUTTON_DOWN
+        - BUTTON_LEFT
+        - BUTTON_RIGHT
+
+  cabot_serial:
+    ros__parameters:
+      port: /dev/ttyESP32
+
+  odriver_adapter_node:
+    ros__parameters:
+      bias: 0.14 # same with cabot3-i1
+      gain_omega: 0.0
+      max_acc: 0.6
+
+  cabot_can:
+    ros__parameters:
+      can_interface: can0
+      # calibration_params: [0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0]

cabot_base/config/helios/helios.yaml

@@ -0,0 +1,26 @@
+common:
+  msg_source: 1                                         #0: not use Lidar
+                                                        #1: packet message comes from online Lidar
+                                                        #2: packet message comes from ROS or ROS2
+                                                        #3: packet message comes from Pcap file
+  send_packet_ros: false                                #true: Send packets through ROS or ROS2(Used to record packet)
+  send_point_cloud_ros: true                            #true: Send point cloud through ROS or ROS2
+lidar:
+  - driver:
+      lidar_type: RSHELIOS         #LiDAR type - RS16, RS32, RSBP, RSHELIOS, RSHELIOS_16P, RS128, RS80, RS48, RSP128, RSP80, RSP48, 
+                                   #             RSM1, RSM1_JUMBO, RSM2, RSM3, RSE1, RSMX.
+      msop_port: 6699              #Msop port of lidar
+      difop_port: 7788             #Difop port of lidar
+      start_angle: 0               #Start angle of point cloud
+      end_angle: 360               #End angle of point cloud 
+      wait_for_difop: true
+      min_distance: 0.2            #Minimum distance of point cloud
+      max_distance: 200            #Maximum distance of point cloud
+      use_lidar_clock: false       #True--Use the lidar clock as the message timestamp
+                                   #False-- Use the system clock as the timestamp                        
+      pcap_path: /home/robosense/lidar.pcap #The path of pcap file
+    ros:
+      ros_frame_id: velodyne                           #Frame id of packet message and point cloud message
+      ros_recv_packet_topic: /rslidar_packets          #Topic used to receive lidar packets from ROS
+      ros_send_packet_topic: /rslidar_packets          #Topic used to send lidar packets through ROS
+      ros_send_point_cloud_topic: /rslidar_points      #Topic used to send point cloud through ROS