Merge pull request #279 from UHHRobotics22-23/doc/hardware

Doc/hardware

marimbabot_hardware/README.md

@@ -0,0 +1,76 @@
+# Marimababot Hardware
+This package defines the ROS driver for the two mallet hardware.
+Three central versions of the driver exist
+- Wifi - The current version using Wifi and UDP to communicate with the holder
+- Serial - First version using a serial (USB) connection to communicate with the device
+- Dummy - Used in simulations or for debugging
+
+Each of these versions has associated source, header and launch files identified by the suffix.
+
+## General Design
+Generally every version is composed of two source files servo_interface_XXXX.cpp and mallet_hardware_control_node_XXXX.cpp in the [src](src/) folder.
+
+We use the packages [controller_manager](http://wiki.ros.org/controller_manager) and [ros_controllers](http://wiki.ros.org/ros_controllers) to implement a [ros_control](http://wiki.ros.org/ros_control) (standard) control interface with a standard joint_trajectory_controller from ros_controllers. See the [github repository of ros_controllers](https://github.com/ros-controls/ros_controllers).
+
+The [guide on rosroboticslearning](https://www.rosroboticslearning.com/ros-control) gives a good starting point for understanding what has to be done, but additionally reading the source code of the packages and looking for other examples is necessary.
+
+### Implementation
+Consequently we only have to implement the hardware_interface::RobotHW (See the figure in the [ros_control wiki page](http://wiki.ros.org/ros_control)).
+It is defined in the servo_interface_XXXX.cpp (and related header file).
+The servo_interface_XXXX.cpp contains the main part of the implementation and facilitates the communication with the device and the handling of connection interuptions.
+The hardware interface is defined as a position based controller (the servo is controlled by the angle) and gives the currently commanded position as the feedback. The commanded position is retrieved from the device but no measurements of the actual position are employed.
+
+The mallet_hardware_control_node_XXXX.cpp initializes the ROS node and loads the parameters given to it. The ServoInterface implemented in servo_interface_XXXX.cpp is started here.
+The control node further runs the controller manager update loop and calls the read and write functions of the ServoInterface.
+
+### Launch files
+The components are initialized in a launch file. The files in [launch](launch/) give examples for debugging purpuses.
+Centrally the loading of the configuration yaml files and the parameters of the controller_spawner are important to the implementation.
+
+### Configuration yaml
+The [controllers.yaml](config/controllers.yaml) file sets the parameters for the standard controllers of the [ros_controllers](http://wiki.ros.org/ros_controllers) package. Important is the definition of the mallet_finger joint for the trajectory_controller.
+
+[joint_limits.yaml](config/joint_limits.yaml) defines the speed, acceleration and position limits of the mallet_finger joint. These are ensured by a check in the ServoInterface write function.
+
+## Version Specific Usage
+
+The differing versions of the ServoInterface have specific configuration parameters and precoditions for normal operation.
+
+### Wifi
+The main implementation of the ServoInterface.
+For normal operation the controlling computer needs to be connected to the wifi of the two mallet holder controller (Marimbabot_Mallet - PW: 12345678).
+
+Two configuration options have to be set to connect to the UDP server running on the microcontroller.
+- address: The ip of the UDP server (default: 192.168.42.1)
+- port: The port of the UDP server (default: 8888)
+
+To change the correct values of these parameters the firmware on the microcontroller has to be adjusted. The firmware is found in the [hardware_design](https://github.com/UHHRobotics22-23/hardware_design/tree/main/arduino_code/mallet_play_servo_wifi) repository.
+
+### Serial
+The serial hardware_interface was first developed and later replaced by the wireless wifi version.
+The serial version needs a different version of the firmware flashed onto the microcontroller found in the [hardware_design](https://github.com/UHHRobotics22-23/hardware_design/tree/main/arduino_code/mallet_play_servo) repository.
+The microcontroller has to be connected over usb to the controlling computer.
+
+Two parameters have to be set:
+- device: The device path in the unix file system (default: /dev/ttyACM0 or /dev/ttyUSB0)
+- baud: The baud rate of the serial connection (default: 115200)
+
+The servo interface can tollerate the device not being present at the start without crashing. In some cases the permissions of a new device have to be adjusted to allow the node to access the device.
+
+    sudo chmod 666 /dev/ttyACM0
+
+### Dummy
+The dummy hardware_interface is built to emulate an actual device responding to the commands given by ros.
+It is mainly used to facilitate the simulation of the project.
+
+No parameters have to be set to use the interface.
+
+## Debug
+The launchfiles in [launch](launch/) are used to create a simple testing setup in which the mallet holder is know to ROS. It uses the dummy robot model [two_mallet_holder.urdf](urdf/two_mallet_holder.urdf) (no practical significance).
+
+Further the [scripts](scripts/) folder contains the two scripts [joint_state.py](scripts/joint_state.py) and [wifi_client_sim.py](scripts/wifi_client_sim.py).
+
+[joint_state.py](scripts/joint_state.py) publishes a dummy position of the two mallet holder which is always in the zero position. It can be used when the mallet holder is included in a bigger model but the control nodes are not running to make the system able to plan.
+The [dummy control node](src/mallet_hardware_control_node_dummy.cpp) is a more realistic solution to the problem.
+
+[wifi_client_sim.py](scripts/wifi_client_sim.py) is a simple "emulator" of a running two mallet assembly to test the [wifi control node](src/mallet_hardware_control_node_wifi.cpp) against. (Note: It hasn't been used for some time and the protocol might not be implemented 100% correctly anymore)

marimbabot_hardware/include/marimbabot_hardware/servo_interface_dummy.hpp

@@ -1,3 +1,7 @@
+/*
+ * Dummy servo interface for testing and simulation for one servo.
+ * Tries to emulate the behaviour of the serial servo interface.
+ */
 #ifndef MARIMBABOT_SERVO_INTERFACE_HPP
 #define MARIMBABOT_SERVO_INTERFACE_HPP
 
@@ -9,9 +13,11 @@
 #include <joint_limits_interface/joint_limits_rosparam.h>
 
 
+// Class definition of the hardware interface for a single dummy servo motor for use with ros_controllers
 class ServoInterface : public hardware_interface::RobotHW {
 
 private:
+    // The struct to hold the state of the servo motor which will be registered to the hardware interface
     struct ServoState {
         std::string name = "";
         double command = -1;    // -1 means no command yet
@@ -25,26 +31,38 @@ class ServoInterface : public hardware_interface::RobotHW {
 public:
     ServoInterface(ros::NodeHandle& node_handle);
 
+    // The read function is called by the main node to read the state of the servo motor
     void read();
+    // The write function is called by the main node to write the command to the servo motor
     void write();
+    // The initialize function is used to ensure a read is performed before any other action
     void initialize();
 
+    // The get_time and get_period functions are used by the controller manager to keep track of time
     const ros::Time& get_time() { return last_run_time; };
     const ros::Duration& get_period() { return last_run_period; };
 
 private:
+    // The joint state interface is used to register the servo state to the controller manager
     hardware_interface::JointStateInterface joint_state_interface;
+    // The position joint interface is used to register the servo command to the controller manager
     hardware_interface::PositionJointInterface position_joint_interface;
+    // The joint limits are used to ensure the servo command is within the limits of the servo
     joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface;
     joint_limits_interface::JointLimits servo_limits;
+    // The state of the servo motor
     ServoState servo_state;
 
+    // The time of the last read and the period between reads
     ros::Time last_run_time;
     ros::Duration last_run_period;
+    // The previous command sent to the servo motor
     double previous_command = -1;
+    // The limits of the servo as given by the l command
     int top_limit = -1;
     int bottom_limit = -1;
     int resolution = -1;
+    // The limit of the servo mapped to radians from 0 to radian_limit
     double radian_limit = -1;
 };
 

marimbabot_hardware/include/marimbabot_hardware/servo_interface_serial.hpp

@@ -1,3 +1,8 @@
+/*
+ * The header file for the ServoInterface class.
+ * This is a hardware interface for a single servo motor.
+ * It is used to communicate with the Arduino over serial.
+ */
 #ifndef MARIMBABOT_SERVO_INTERFACE_HPP
 #define MARIMBABOT_SERVO_INTERFACE_HPP
 
@@ -10,9 +15,11 @@
 #include <serial/serial.h>
 
 
+// Class definition of the hardware interface for a single servo motor for use with ros_controllers
 class ServoInterface : public hardware_interface::RobotHW {
 
 private:
+    // The struct to hold the state of the servo motor which will be registered to the hardware interface
     struct ServoState {
         std::string name = "";
         double command = -1;    // -1 means no command yet
@@ -26,30 +33,45 @@ class ServoInterface : public hardware_interface::RobotHW {
 public:
     ServoInterface(ros::NodeHandle& node_handle, std::string &device, int baud);
 
+    // The read function is called by the main node to read the state of the servo motor
     void read();
+    // The write function is called by the main node to write the command to the servo motor
     void write();
+    // The initialize function is used to ensure a read is performed before any other action
     void initialize();
 
+    // The get_time and get_period functions are used by the controller manager to keep track of time
     const ros::Time& get_time() { return last_run_time; };
     const ros::Duration& get_period() { return last_run_period; };
 
 private:
+    // The joint state interface is used to register the servo state to the controller manager
     hardware_interface::JointStateInterface joint_state_interface;
+    // The position joint interface is used to register the servo command to the controller manager
     hardware_interface::PositionJointInterface position_joint_interface;
+    // The joint limits are used to ensure the servo command is within the limits of the servo
     joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface;
     joint_limits_interface::JointLimits servo_limits;
+    // The state of the servo motor
     ServoState servo_state;
 
+    // The time of the last read and the period between reads
     ros::Time last_run_time;
     ros::Duration last_run_period;
+    // The serial object used to communicate with the Arduino
     serial::Serial arduino_serial;
+    // A flag for defining the last error code for the Ardunio connection to prevent double error prints
     int last_arduino_error = 0;
+    // The previous command sent to the servo motor
     double previous_command = -1;
+    // The limits of the servo as given by the l command
     int top_limit = -1;
     int bottom_limit = -1;
     int resolution = -1;
+    // The limit of the servo mapped to radians from 0 to radian_limit
     double radian_limit = -1;
 
+    // A internal function used to try opening the serial port
     bool try_open_serial_port();
 };
 

marimbabot_hardware/include/marimbabot_hardware/servo_interface_wifi.hpp

@@ -1,3 +1,8 @@
+/*
+ * The header file for the ServoInterface class.
+ * This is a hardware interface for a single servo motor.
+ * It is used to communicate with the Arduino over wifi.
+ */
 #ifndef MARIMBABOT_SERVO_INTERFACE_HPP
 #define MARIMBABOT_SERVO_INTERFACE_HPP
 
@@ -23,10 +28,11 @@
 #define MAXLINE 1024
 //#include <serial/serial.h>
 
-
+// Class definition of the hardware interface for a single servo motor for use with ros_controllers
 class ServoInterface : public hardware_interface::RobotHW {
 
 private:
+    // The struct to hold the state of the servo motor which will be registered to the hardware interface
     struct ServoState {
         std::string name = "";
         double command = -1;    // -1 means no command yet
@@ -41,50 +47,63 @@ class ServoInterface : public hardware_interface::RobotHW {
 public:
     ServoInterface(ros::NodeHandle& node_handle, std::string &address, int port); //device
 
+    // The read function is called by the main node to read the state of the servo motor
     void read();
+    // The write function is called by the main node to write the command to the servo motor
     void write();
+    // The initialize function is used to ensure a read is performed before any other action
     void initialize();
-
+    // The get_time and get_period functions are used by the controller manager to keep track of time
     const ros::Time& get_time() { return last_run_time; };
     const ros::Duration& get_period() { return last_run_period; };
 
 private:
+    // The joint state interface is used to register the servo state to the controller manager
     hardware_interface::JointStateInterface joint_state_interface;
+    // The position joint interface is used to register the servo command to the controller manager
     hardware_interface::PositionJointInterface position_joint_interface;
+    // The joint limits are used to ensure the servo command is within the limits of the servo
     joint_limits_interface::PositionJointSaturationInterface position_joint_saturation_interface;
     joint_limits_interface::JointLimits servo_limits;
+    // The state of the servo motor
     ServoState servo_state;
-
+    // The time of the last read and the period between reads
     ros::Time last_run_time;
     ros::Duration last_run_period;
-
+    // A flag for defining the last error code for the Ardunio connection to prevent double error prints
     int last_arduino_error = 0;
+     // The previous command sent to the servo motor
     double previous_command = -1;
+     // The limits of the servo as given by the l command
     int top_limit = -1;
     int bottom_limit = -1;
     int resolution = -1;
+     // The limit of the servo mapped to radians from 0 to radian_limit
     double radian_limit = -1;
+    // Checks if the limit is already stored
     bool limit_checker = 0;
-
+    // Socket for UDP Connection
     int sockfd;
     char buffer[MAXLINE];
-    char const *l_Sender = "l\n";
-    char const *p_Sender = "p\n";
-
-
     struct sockaddr_in servaddr, cliaddr;
-
     socklen_t len;
     int received_message;
 
+    // Special char to trigger the limits and the position from the arduino when send via udp
+    char const *l_Sender = "l\n";
+    char const *p_Sender = "p\n";
+
+
+    // A internal function used to try opening the udp port
     void try_open_udp_port();
+    //convert string to char
     char* convert_to_char(std::string str);
+    //Sends a char via UDP to the arduino and receives the message from the arduino
     void send_and_receive(const char *);
+    //processes the message which contains limits.
     void receive_limits_function(std::string response);
+    //processes the message which contains positions.
     void receive_postion_function(std::string response);
-    void receive_changeposition_function(std::string response);
-
-
 };
 
 #endif // MARIMBABOT_SERVO_INTERFACE_HPP

marimbabot_hardware/launch/mallet_servo_control_debug_dummy.launch

@@ -1,14 +1,21 @@
+<!-- A debug launch file for isolated testing of the dummy servo -->
 <launch>
+    <!-- Using a group to prevent collisions -->
     <group ns="MalletHolder">
+        <!-- Loading the controllers and joint limits from the config files -->
         <rosparam file="$(find marimbabot_hardware)/config/controllers.yaml" command="load"/>
         <rosparam file="$(find marimbabot_hardware)/config/joint_limits.yaml" command="load"/>
 
         <!-- ! This is only for demo setups START -->
+        <!-- Uses a dummy URDF to make the mallet_finger joint known to the system -->
         <!-- <param name="robot_description" command="cat $(find marimbabot_hardware)/urdf/two_mallet_holder.urdf"/>
         <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/> -->
         <!-- ! This is only for demo setups END -->
 
+        <!-- Starting the hardware control node -->
         <node name="mallet_hardware_control_node" pkg="marimbabot_hardware" type="mallet_hardware_control_node_dummy" output="screen" />
+
+        <!-- Spawning the controllers according to the config -->
         <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
             args="
                 joint_state_controller

marimbabot_hardware/launch/mallet_servo_control_debug_serial.launch

@@ -1,18 +1,25 @@
+<!-- A debug launch file for isolated testing of the serial based servo hardware interface -->
 <launch>
+    <!-- Using a group to prevent collisions -->
     <group ns="MalletHolder">
+        <!-- Loading the controllers and joint limits from the config files -->
         <rosparam file="$(find marimbabot_hardware)/config/controllers.yaml" command="load"/>
         <rosparam file="$(find marimbabot_hardware)/config/joint_limits.yaml" command="load"/>
 
         <!-- ! This is only for demo setups START -->
+        <!-- Uses a dummy URDF to make the mallet_finger joint known to the system -->
         <param name="robot_description" command="cat $(find marimbabot_hardware)/urdf/two_mallet_holder.urdf"/>
         <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
         <!-- ! This is only for demo setups END -->
 
+        <!-- Starting the hardware control node -->
         <node name="mallet_hardware_control_node" pkg="marimbabot_hardware" type="mallet_hardware_control_node" output="screen">
             <!-- Setting device parameter -->
             <param name="device" value="/dev/ttyACM0"/>
             <param name="baudrate" value="115200"/>
         </node>
+
+        <!-- Spawning the controllers according to the config -->
         <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
             args="
                 joint_state_controller

marimbabot_hardware/launch/mallet_servo_control_debug_wifi.launch

@@ -1,19 +1,25 @@
+<!-- This launch file is for debugging the mallet servo control over wifi -->
 <launch>
+    <!-- Using a group to prevent collisions -->
     <group ns="MalletHolder">
+        <!-- Loading the controllers and joint limits from the config files -->
         <rosparam file="$(find marimbabot_hardware)/config/controllers.yaml" command="load"/>
         <rosparam file="$(find marimbabot_hardware)/config/joint_limits.yaml" command="load"/>
 
         <!-- ! This is only for demo setups START -->
+        <!-- Uses a dummy URDF to make the mallet_finger joint known to the system -->
         <param name="robot_description" command="cat $(find marimbabot_hardware)/urdf/two_mallet_holder.urdf"/>
         <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
         <!-- ! This is only for demo setups END -->
 
+        <!-- Starting the hardware control node -->
         <node name="mallet_hardware_control_node" pkg="marimbabot_hardware" type="mallet_hardware_control_node_wifi" output="screen">
             <!-- Setting device parameter -->
             <param name="address" value="192.168.42.1"/>
             <param name="port" value="8888"/>
         </node>
 
+        <!-- Spawning the controllers according to the config -->
         <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen"
             args="
                 joint_state_controller

marimbabot_hardware/scripts/joint_state.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python
 
+# This is a simple joint state publisher for the mallet_finger joint. Always publishes 0.0 radian.
+
 import rospy
 from sensor_msgs.msg import JointState
 

marimbabot_hardware/scripts/wifi_client_sim.py

@@ -1,3 +1,6 @@
+# A simple script to simulate the wifi client on the marimbabot
+# Used mainly for debugging
+
 import socket
 
 UDP_IP = "127.0.0.1"