agh-space-systems-rover · kapi117 · Oct 24, 2024 · Oct 24, 2024 · Oct 25, 2024 · Oct 25, 2024
diff --git a/docs/arm_control.drawio.svg b/docs/arm_control.drawio.svg
diff --git a/docs/gs_dark_autonomy.png b/docs/gs_dark_autonomy.png
diff --git a/docs/gs_light.png b/docs/gs_light.png
diff --git a/docs/gs_settings.png b/docs/gs_settings.png
diff --git a/kalman_arm_config/CMakeLists.txt b/kalman_arm_config/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 install(
-  DIRECTORY config launch predefined_poses predefined_trajectories rviz
+  DIRECTORY config launch predefined_poses predefined_trajectories
   DESTINATION share/${PROJECT_NAME}
 )
 

diff --git a/kalman_arm_config/README.md b/kalman_arm_config/README.md
@@ -0,0 +1,24 @@
+# kalman_arm_config
+
+This package contains configuration files for the arm of AGH Space Systems' planetary rover Kalman. In this folder there are no source files, only configuration files and launch files.
+
+## Description
+
+```
+kalman_arm_config
+├── config
+│   ├── arm_controller.yaml             # Configuration file for the arm controllers (velocity and joint trajectory)
+│   ├── cyclonedds.xml                  # Configuration file for the CycloneDDS on the arm
+│   ├── nmea_navsat_driver.yaml         # Configuration file for the NMEA GPS driver
+|   ├── phidgets_spatial.yaml           # Configuration file for the Phidgets Spatial driver (IMU)
+|   ├── predefined_poses.yaml           # Configuration file for predefined arm poses (thresholds and poses list)
+|   ├── predefined_trajectories.yaml    # Configuration file for predefined arm trajectories (thresholds and trajectories list)
+|   ├── ros_link.yaml                   # Configuration file for the ROS link for data serialization
+|   ├── servo_config.yaml               # Configuration file for the MoveIt servo controller
+├── launch
+│   ├── arm_controller.launch.py        # Launch file for the arm controllers, ROS2 control, state publisher and broadcaster
+│   ├── drivers.launch.py               # Launch file for the arm drivers (Phidgets Spatial, NMEA GPS)
+│   ├── master.launch.py                # Launch file for the master communications, ROS link and utils
+│   ├── servo.launch.py                 # Launch file for the MoveIt servo controller and servo parameter setter
+|   ├── trajectories.launch.py          # Launch file for the trajectory and pose executors
+```
diff --git a/kalman_arm_config/package.xml b/kalman_arm_config/package.xml
@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>kalman_arm_config</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="[email protected]">gs</maintainer>
-  <license>TODO: License declaration</license>
+  <version>1.0.0</version>
+  <description>Configuration files for Kalman's arm</description>
+  <maintainer email="[email protected]">Kacper Iwicki</maintainer>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -15,4 +15,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
diff --git a/kalman_arm_controller/README.md b/kalman_arm_controller/README.md
@@ -1,3 +1,15 @@
 # kalman_arm_controller
 
-ROS package for controlling Kalman's arm. It includes CAN related library to interface with joints as well as MoveIt controllers.
+ROS package for controlling Kalman's arm. It includes CAN related library to interface with joints as well as MoveIt controllers.
+
+## Description
+
+The main element of this package is hardware controller used by ROS2 control to interface with the arm joints. For interfacing it uses CAN FD protocol.
+
+Additionally in this pacakge there are some scripts used for testing and providing extra functionalities.
+
+## Kalman's Arm Description
+
+![](../docs/arm_control.drawio.svg)
+
+Above you can find a simple diagram showing data flow in arm. Green blocks represent code that is written for custom usage of ROS2 Control.
diff --git a/kalman_arm_controller/package.xml b/kalman_arm_controller/package.xml
@@ -2,16 +2,12 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>kalman_arm_controller</name>
-  <version>0.0.0</version>
-  <description>Demo for 6 DOF robot.</description>
+  <version>1.0.0</version>
+  <description>Controller implementation for Kalman's arm</description>
 
-  <maintainer email="[email protected]">Dr.-Ing. Denis Štogl</maintainer>
-  <maintainer email="[email protected]">Bence Magyar</maintainer>
-  <maintainer email="[email protected]">Christoph Froehlich</maintainer>
+  <maintainer email="[email protected]">Kacper Iwicki</maintainer>
 
-  <author email="[email protected]">Paul Gesel</author>
-
-  <license>Apache-2.0</license>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -44,4 +40,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
diff --git a/kalman_arm_moveit_config/README.md b/kalman_arm_moveit_config/README.md
@@ -0,0 +1,3 @@
+# kalman_arm_moveit_config
+
+This package contains MoveIt configuration for the arm of AGH Space Systems' planetary rover Kalman. In this folder there are no source files, only configuration files and launch files. Most of them are auto-generated by MoveIt setup assistant.
diff --git a/kalman_arm_moveit_config/package.xml b/kalman_arm_moveit_config/package.xml
@@ -3,18 +3,18 @@
 <package format="3">
   <name>kalman_arm_moveit_config</name>
   <version>0.3.0</version>
-  <description>
-     An automatically generated package with all the configuration and launch files for using the arm with the MoveIt Motion Planning Framework
-  </description>
-  <maintainer email="[email protected]">todo</maintainer>
+  <description> An automatically generated package with all the configuration and launch files for
+    using the arm with the MoveIt Motion Planning Framework </description>
+  <maintainer email="[email protected]">Kacper Iwicki</maintainer>
 
   <license>BSD</license>
 
   <url type="website">http://moveit.ros.org/</url>
   <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
   <url type="repository">https://github.com/ros-planning/moveit2</url>
 
-  <author email="[email protected]">todo</author>
+  <author email="[email protected]">Kacper Iwicki
+  </author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -45,6 +45,6 @@
 
 
   <export>
-      <build_type>ament_cmake</build_type>
+    <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
diff --git a/kalman_arm_utils/CMakeLists.txt b/kalman_arm_utils/CMakeLists.txt
@@ -21,7 +21,11 @@ install(PROGRAMS
   DESTINATION lib/${PROJECT_NAME}
 )
 
-add_executable(joint_republisher src/joint_republisher.cpp)
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+add_executable(joint_republisher src/gs/joint_republisher.cpp)
 target_include_directories(joint_republisher PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
@@ -33,7 +37,7 @@ install(TARGETS joint_republisher
   DESTINATION lib/${PROJECT_NAME})
 
 
-add_executable(gripper_republisher src/gripper_republisher.cpp)
+add_executable(gripper_republisher src/gs/gripper_republisher.cpp)
 target_include_directories(gripper_republisher PUBLIC
 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 $<INSTALL_INTERFACE:include>)
@@ -45,7 +49,7 @@ install(TARGETS gripper_republisher
 DESTINATION lib/${PROJECT_NAME})  
 
 
-add_executable(arm_state_republisher src/arm_state_republisher.cpp)
+add_executable(arm_state_republisher src/gs/arm_state_republisher.cpp)
 target_include_directories(arm_state_republisher PUBLIC
 $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
 $<INSTALL_INTERFACE:include>)

diff --git a/kalman_arm_utils/README.md b/kalman_arm_utils/README.md
@@ -0,0 +1,24 @@
+# kalman_arm_utils
+
+This package contains utilities for integration and extra functionalities for Kalman's arm. Some scripts are run on groundstation, some on the rover.
+
+## Description
+
+```
+kalman_arm_utils
+├── scripts                         # Scripts for launching Python nodes
+├── kalman_arm_utils                # Python package with utilities
+│   ├── __init__.py
+|   ├── arm
+|   |   ├── fake_move_server.py         # Fake MoveIt server for capturing trajectories
+|   |   ├── pose_requester.py           # Script for requesting predefined poses
+|   |   ├── trajectory_requester.py     # Script for requesting predefined trajectories
+|   |   ├── servo_param_setter_node.py  # Script for setting MoveIt servo parameters from topics
+├── src                                 # Source files for C++ utilities
+│   ├── arm                             # C++ utilities for arm
+│   │   ├── arm_state_publisher.cpp     # Node for publishing arm state in custom format
+│   ├── gs                              # C++ utilities for groundstation
+│   │   ├── arm_state_republisher.cpp   # Node for republishing arm state from custom format
+│   │   ├── gripper_republisher.cpp     # Node for republishing gripper command from inputs
+│   │   ├── joint_republisher.cpp       # Node for republishing joint command from inputs
+```
diff --git a/kalman_arm_utils/kalman_arm_utils/arm/__init__.py b/kalman_arm_utils/kalman_arm_utils/arm/__init__.py
diff --git a/...tils/kalman_arm_utils/fake_move_server.py → .../kalman_arm_utils/arm/fake_move_server.py b/...tils/kalman_arm_utils/fake_move_server.py → .../kalman_arm_utils/arm/fake_move_server.py
diff --git a/...man_arm_utils/pose_request_sender_node.py → ...ls/kalman_arm_utils/arm/pose_requester.py b/...man_arm_utils/pose_request_sender_node.py → ...ls/kalman_arm_utils/arm/pose_requester.py
diff --git a/...lman_arm_utils/servo_param_setter_node.py → ..._arm_utils/arm/servo_param_setter_node.py b/...lman_arm_utils/servo_param_setter_node.py → ..._arm_utils/arm/servo_param_setter_node.py
diff --git a/...alman_arm_utils/trajectory_sender_node.py → ...man_arm_utils/arm/trajectory_requester.py b/...alman_arm_utils/trajectory_sender_node.py → ...man_arm_utils/arm/trajectory_requester.py
diff --git a/kalman_arm_utils/launch/gs.launch.py b/kalman_arm_utils/launch/gs.launch.py
@@ -0,0 +1,23 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            Node(
+                package="kalman_arm_utils",
+                executable="joint_republisher",
+                name="joint_republisher",
+            ),
+            Node(
+                package="kalman_arm_utils",
+                executable="gripper_republisher",
+                parameters=[{"gripper_scale": 25.0}],
+            ),
+            Node(
+                package="kalman_arm_utils",
+                executable="arm_state_republisher",
+            ),
+        ]
+    )
diff --git a/kalman_arm_utils/package.xml b/kalman_arm_utils/package.xml
@@ -2,10 +2,10 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>kalman_arm_utils</name>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="kacper.iwi@gmail.com">root</maintainer>
-  <license>TODO: License declaration</license>
+  <version>1.0.0</version>
+  <description>Utilities for integration and features for Kalman's arm</description>
+  <maintainer email="[email protected].com">Kacper Iwicki</maintainer>
+  <license>MIT</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 
@@ -19,4 +19,4 @@
   <export>
     <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
diff --git a/kalman_arm_utils/scripts/pose_request_sender b/kalman_arm_utils/scripts/pose_request_sender
@@ -1,5 +1,5 @@
 #!/usr/bin/python3
-from kalman_arm_utils.pose_request_sender_node import main
+from kalman_arm_utils.arm.pose_requester import main
 
 if __name__ == '__main__':
     main()
diff --git a/kalman_arm_utils/scripts/servo_param_setter b/kalman_arm_utils/scripts/servo_param_setter
@@ -1,5 +1,5 @@
 #!/usr/bin/python3
-from kalman_arm_utils.servo_param_setter_node import main
+from kalman_arm_utils.arm.servo_param_setter_node import main
 
 if __name__ == '__main__':
     main()
diff --git a/kalman_arm_utils/scripts/trajectory_sender b/kalman_arm_utils/scripts/trajectory_sender
@@ -1,5 +1,5 @@
 #!/usr/bin/python3
-from kalman_arm_utils.trajectory_sender_node import main
+from kalman_arm_utils.arm.trajectory_requester import main
 
 if __name__ == '__main__':
     main()
diff --git a/...n_arm_utils/src/arm_state_republisher.cpp → ...rm_utils/src/gs/arm_state_republisher.cpp b/...n_arm_utils/src/arm_state_republisher.cpp → ...rm_utils/src/gs/arm_state_republisher.cpp
diff --git a/kalman_arm_utils/src/gripper_republisher.cpp → ..._arm_utils/src/gs/gripper_republisher.cpp b/kalman_arm_utils/src/gripper_republisher.cpp → ..._arm_utils/src/gs/gripper_republisher.cpp
diff --git a/kalman_arm_utils/src/joint_republisher.cpp → ...an_arm_utils/src/gs/joint_republisher.cpp b/kalman_arm_utils/src/joint_republisher.cpp → ...an_arm_utils/src/gs/joint_republisher.cpp
diff --git a/kalman_aruco/README.md b/kalman_aruco/README.md
@@ -0,0 +1,3 @@
+# kalman_aruco
+
+This package includes configuration and launch file for the ArUco markers detection and pose estimation. It uses the `aruco_opencv` package for detection.
diff --git a/kalman_bringup/README.md b/kalman_bringup/README.md
@@ -0,0 +1,7 @@
+# kalman_bringup
+
+This package contains launch files for starting the whole system.
+
+## Idea
+
+Each launch uses `_commons.launch.py` file to include common parameters and nodes. One can create new launch file for specific use case by including `_commons.launch.py` with appropriate parameters and adding new nodes.
diff --git a/kalman_bringup/launch/_commons.launch.py b/kalman_bringup/launch/_commons.launch.py
@@ -77,6 +77,7 @@ def get_str(name):
                     "joint_state_publisher_gui": get_str(
                         "description.joint_state_publisher_gui"
                     ),
+                    "with_arm": get_str("description.with_arm"),
                 }.items(),
             )
         ]
@@ -115,6 +116,12 @@ def get_str(name):
                     ),
                     "master": get_str("hardware.master"),
                     "master.mode": get_str("hardware.master.mode"),
+                    "master.autonomy": get_str("hardware.master.autonomy"),
+                    "master.drivers.arm": get_str("hardware.master.drivers.arm"),
+                    "master.drivers.ueuos": get_str("hardware.master.drivers.ueuos"),
+                    "master.drivers.feed": get_str("hardware.master.drivers.feed"),
+                    "master.drivers.tunnel": get_str("hardware.master.drivers.tunnel"),
+                    "master.drivers.drill": get_str("hardware.master.drivers.drill"),
                     "rgbd_ids": get_str("hardware.rgbd_ids"),
                     "imu": get_str("hardware.imu"),
                     "compass_calibration": get_str("hardware.compass_calibration"),
@@ -283,6 +290,19 @@ def get_str(name):
             ),
         ]
 
+    if get_bool("arm_utils"):
+        description += [
+            IncludeLaunchDescription(
+                PythonLaunchDescriptionSource(
+                    str(
+                        get_package_share_path("kalman_arm_utils")
+                        / "launch"
+                        / "gs.launch.py"
+                    )
+                ),
+            ),
+        ]
+
     if get_bool("gs"):
         description += [
             IncludeLaunchDescription(
@@ -331,6 +351,11 @@ def generate_launch_description():
                 default_value="false",
                 description="Start up state publishers for the robot description.",
             ),
+            DeclareLaunchArgument(
+                "description.with_arm",
+                default_value="false",
+                description="Start up the robot description with the arm.",
+            ),
             DeclareLaunchArgument(
                 "description.joint_state_publisher_gui",
                 default_value="false",
@@ -371,6 +396,36 @@ def generate_launch_description():
                 default_value="pc",
                 description="Run master drivers in 'pc', 'gs' or 'arm' mode.",
             ),
+            DeclareLaunchArgument(
+                "hardware.master.autonomy",
+                default_value="false",
+                description="Start the master autonomy related nodes.",
+            ),
+            DeclareLaunchArgument(
+                "hardware.master.drivers.arm",
+                default_value="false",
+                description="Start the arm driver.",
+            ),
+            DeclareLaunchArgument(
+                "hardware.master.drivers.ueuos",
+                default_value="false",
+                description="Start the UEUOS driver.",
+            ),
+            DeclareLaunchArgument(
+                "hardware.master.drivers.feed",
+                default_value="false",
+                description="Start the feed driver.",
+            ),
+            DeclareLaunchArgument(
+                "hardware.master.drivers.tunnel",
+                default_value="false",
+                description="Start the tunnel client.",
+            ),
+            DeclareLaunchArgument(
+                "hardware.master.drivers.drill",
+                default_value="false",
+                description="Start the drill driver.",
+            ),
             DeclareLaunchArgument(
                 "hardware.imu",
                 default_value="false",
@@ -541,6 +596,11 @@ def generate_launch_description():
                 default_value="false",
                 description="Deactivate ArUco detection nodes when supervisor is not actively looking for tags.",
             ),
+            DeclareLaunchArgument(
+                "arm_utils",
+                default_value="false",
+                description="Start up the arm utilities.",
+            ),
             DeclareLaunchArgument(
                 "gs",
                 default_value="false",

diff --git a/kalman_bringup/launch/erc_autonomy_gs.launch.py b/kalman_bringup/launch/erc_autonomy_gs.launch.py
@@ -21,6 +21,8 @@ def generate_launch_description():
                     "hardware": "true",
                     "hardware.master": "true",  # WARNING: Having two master drivers enabled on the same network will cause messages on topics shared via RF to loop around over network and cause a positive feedback loop! Remember to disable master here or disconnect it physically when connecting to the robot over network.
                     "hardware.master.mode": "gs",
+                    "hardware.master.autonomy": "true",
+                    "hardware.master.drivers.tunnel": "true",
                     "gs": "true",
                     "wheels": "true",
                 }.items(),

diff --git a/kalman_bringup/launch/erc_autonomy_rover.launch.py b/kalman_bringup/launch/erc_autonomy_rover.launch.py
@@ -21,6 +21,8 @@ def generate_launch_description():
                     "hardware.rgbd_ids": "d455_front d455_back d455_left d455_right",
                     "hardware.master": "true",
                     "hardware.master.mode": "pc",
+                    "hardware.master.autonomy": "true",
+                    "hardware.master.drivers.ueuos": "true",
                     "hardware.imu": "true",
                     "clouds": "true",
                     "clouds.rgbd_ids": "d455_front d455_back d455_left d455_right",