diff --git a/docs/arm_hardware_motion_architecture.md b/docs/arm_hardware_motion_architecture.md new file mode 100644 index 0000000..00292ef --- /dev/null +++ b/docs/arm_hardware_motion_architecture.md @@ -0,0 +1,259 @@ +# Arm Hardware and Motion Architecture + +## Goal + +Use the same four-joint arm model in RViz for both simulation and a connected +Arduino, while allowing the motion test to command the physical arm and report +failures instead of only animating a simulated pose. + +The current model is `old_arm_prototype.urdf` with these joints: + +- `base_yaw` +- `shoulder` +- `elbow` +- `wrist_roll` + +## Current State + +```text +MotionTestNode -- /joint_states --> robot_state_publisher --> RViz RobotModel + ^ + | +RViz MotionTestPanel -- String RUN/HOME/STOP +``` + +The motion test currently publishes generated positions directly to +`/joint_states`. That is suitable for visualization, but it is not a hardware +control path: it represents commanded positions as if they were measured +positions. + +There is currently no Arduino transport, encoder feedback path, +`ros2_control` hardware plugin, trajectory controller, or motion action in the +workspace. + +The archived handoff provides a usable first transport contract: + +- Arduino Uno R4 WiFi at 115200 baud over USB serial. +- Servo signals: D1 `base_yaw`, D2 `shoulder`, D3 `elbow`, D4 `wrist_roll`. +- Hold switch: D7 to ground using `INPUT_PULLUP`. +- Commands: `ID`, `MOVE,s1,s2,s3,s4,durationMs`, `JOG,servo,delta,durationMs`, + and `HOLD`. +- Responses: `READY,IK4,1`, `OK,MOVE`, `OK,JOG`, `OK,ARRIVED`, `OK,HOLD`, + `OK,SWITCH HOLD`, and `ERROR,...`. + +The firmware interpolates commands internally every 20 ms and has software +limits, but it does not stream measured servo angles. Its `OK,ARRIVED` response +means the command timeline completed, not that the mechanism was measured at +the target. + +## Target Runtime Architecture + +```text + command path +RViz MotionTestPanel or motion_test + | + | FollowJointTrajectory action + v +joint_trajectory_controller + | + v +waybionic_hardware (ros2_control SystemInterface) + | + | serial/USB protocol + v +Arduino + servo hardware + | + | command status, health, errors + v +waybionic_hardware + | + +--> /joint_states --> robot_state_publisher --> RViz + | + +--> /diagnostics --> DiagnosticsPanel +``` + +The current Arduino firmware has no encoder or potentiometer feedback. It +reports command acceptance and arrival, but not measured servo positions. In +the first physical integration, the host bridge can publish an estimated +`/joint_states` pose from the accepted command timeline. RViz will then show +the commanded pose, not verified mechanical position. When real feedback is +added, measured state should replace that estimate. + +## Runtime Modes + +### Simulation + +```text +motion_test simulation publisher --> /joint_states --> robot_state_publisher +``` + +This preserves the existing visual motion test. It must not run at the same +time as the physical hardware state publisher. + +### Physical Hardware + +```text +motion_test/action client --> trajectory controller --> Arduino +Arduino command status --> estimated /joint_states --> robot_state_publisher --> RViz +``` + +The launch file should select exactly one mode, for example with a +`hardware_mode` argument whose values are `simulation` and `arduino`. + +## ROS Interfaces + +These are the proposed stable interfaces between packages: + +| Purpose | Interface | +| --- | --- | +| Joint position estimate or measurement | `/joint_states`, `sensor_msgs/msg/JointState` | +| Motion command | `/joint_trajectory_controller/follow_joint_trajectory`, `control_msgs/action/FollowJointTrajectory` | +| Optional lower-level command stream | `/joint_trajectory_controller/joint_trajectory`, `trajectory_msgs/msg/JointTrajectory` | +| Hardware and safety state | `/diagnostics`, `diagnostic_msgs/msg/DiagnosticArray` | + +The motion test should become an action client. Actions provide acceptance, +feedback, completion, cancellation, and failure results, which the current +`RUN`/`HOME`/`STOP` string topic cannot provide. + +The existing string topic can remain temporarily as a simulation-only adapter +while the action path is introduced. + +The first bridge implementation is available as `waybionic_hardware` and +accepts the existing `/old_arm_motion_test/command` topic. It translates +`RUN`, `HOME`, and `STOP` into the firmware protocol, publishes the estimated +pose, and publishes connection/command status on `/diagnostics`. + +The initial and HOME pose is the calibrated upright pose: physical servo +angles `[90.0, 35.0, 151.5, 27.5]`, corresponding to model joint angles +`[0, 90, 0, 0]` degrees. The manual joint-state stepper is opt-in so it does +not overwrite this upright motion-test state by publishing zero positions. + +## Package Responsibilities + +### `waybionic_description` + +- Own the production URDF and joint limits. +- Add a `ros2_control` block for the four actuated joints. +- Keep the physical-to-model calibration in one documented place. + +### New `waybionic_hardware` + +- Own the Arduino transport and packet protocol. +- Convert model radians to calibrated servo commands. +- Convert accepted command state to model radians until real sensors exist. +- Replace estimated state with measured state when encoders or potentiometers + are added. +- Publish hardware diagnostics and connection/watchdog faults. +- Refuse motion when disconnected, stale, out of range, or stopped. + +The first implementation can use a small serial bridge if a full +`ros2_control` plugin is not yet ready. The public ROS interfaces should still +match the target design so the bridge can later be replaced without changing +RViz or the motion test. + +### `waybionic_motion_test` + +- Generate safe, bounded test trajectories. +- Send trajectories through the controller/action interface. +- Verify Arduino acknowledgment and arrival reports for each target. +- Verify measured feedback against each target once sensors are available. +- Abort on action failure, stale feedback, limit violation, or timeout. +- Keep a simulation implementation for development without hardware. + +It must not publish synthetic `/joint_states` in physical mode. + +### `waybionic_rviz_plugins` + +- Keep the existing `RobotModel` visualization. +- Update `MotionTestPanel` to use the action interface. +- Show connection, current joint values, target, progress, and failure reason. +- Keep emergency stop separate from normal test completion. + +### `waybionic_bringup` + +- Select simulation or Arduino mode. +- Start `robot_state_publisher` in both modes. +- Start the simulation publisher only in simulation mode. +- Start hardware and controllers only in Arduino mode. +- Prevent both state publishers from running together. + +## Safety Rules + +1. Start in a verified HOME pose before running a sequence. +2. Enforce URDF position and velocity limits before sending commands. +3. Require a hardware heartbeat and stop on a stale heartbeat. +4. Stop on serial disconnect, malformed feedback, over-current, or an + Arduino-reported fault. +5. Treat the stop command as cancellation plus a hardware stop request; do not + merely stop publishing messages. +6. A latched bridge fault ignores subsequent `READY` messages and can only be + cleared by restarting the bridge; inspect the reported fault before doing so. +7. Require an explicit physical-mode launch argument so a development launch + cannot move the arm accidentally. +8. Keep the first physical test at low speed with one joint at a time before + running synchronized trajectories. + +## Incremental Implementation Plan + +1. **Define the contract:** confirm Arduino transport, feedback availability, + servo calibration, joint limits, and the Arduino packet format. +2. **Separate modes:** add launch arguments and prevent the current simulated + `/joint_states` publisher from starting in physical mode. +3. **Build the transport:** add `waybionic_hardware` with connection state, + heartbeat, command conversion, command-state estimation, and diagnostics. +4. **Add standard control:** connect the transport to `ros2_control` and load a + joint state broadcaster plus trajectory controller. +5. **Convert the test:** send bounded trajectories through the action and + verify Arduino arrival status against each target. Add measured-feedback + verification when sensors become available. +6. **Upgrade the panel:** display action and hardware state, and expose stop + only when the controller reports the arm is connected. +7. **Validate progressively:** test conversion math, then a fake serial device, + then RViz with recorded feedback, and only then the physical arm. + +## Current Bringup Commands + +Simulation remains the default: + +```text +ros2 launch waybionic_bringup ground_station.launch.py +``` + +Arduino mode requires an explicit serial port: + +```text +ros2 launch waybionic_bringup ground_station.launch.py \ + hardware_mode:=arduino arduino_port:=/dev/ttyACM0 +``` + +The bridge can be exercised without an Arduino: + +```text +ros2 launch waybionic_bringup ground_station.launch.py \ + hardware_mode:=arduino arduino_dry_run:=true launch_rviz:=false +``` + +The first physical test should use the external servo-power switch, confirm +the arm is supported and clear, then use `HOME` before `RUN`. The current +bridge intentionally does not claim measured position feedback. + +## Definition of Done + +- RViz follows the Arduino command estimate in the first physical version and + measured feedback once sensors are added. +- The motion test moves the physical arm through a bounded sequence. +- A disconnected or faulted Arduino prevents motion and is visible in the + panel and `/diagnostics`. +- A stop request cancels the active command and leaves the arm in a known + state. +- Simulation still works without an Arduino. +- No node other than the selected state source publishes `/joint_states`. + +## Decisions Still Needed + +- Whether future feedback will be servo command echo, potentiometer/encoder + measurement, or both. The current firmware provides command status only. +- Whether the external servo-power switch is the formal emergency stop. +- Physical emergency-stop wiring and how its state reaches ROS. +- Whether the arm is controlled by hobby servos or motor drivers with a + lower-level controller. \ No newline at end of file diff --git a/firmware/waybionic_uno_r4/waybionic_uno_r4.ino b/firmware/waybionic_uno_r4/waybionic_uno_r4.ino new file mode 100644 index 0000000..2de1589 --- /dev/null +++ b/firmware/waybionic_uno_r4/waybionic_uno_r4.ino @@ -0,0 +1,294 @@ +/* + Uno R4 WiFi - Four-servo IK calibration controller + + Wiring: + Servo 1 signal -> D1 (base yaw) + Servo 2 signal -> D2 (shoulder) + Servo 3 signal -> D3 (elbow) + Servo 4 signal -> D4 (wrist roll) + Hold switch -> D7 and GND (uses INPUT_PULLUP) + + Serial at 115200 baud: + ID + MOVE,s1,s2,s3,s4,durationMs + JOG,servoNumber,deltaDegrees,durationMs + HOLD + + Angles are physical 270-degree servo angles. +*/ + +#include +#include +#include +#include + +const byte SERVO_COUNT = 4; +const byte SERVO_PINS[SERVO_COUNT] = {1, 2, 3, 4}; +const byte HOLD_SWITCH_PIN = 7; + +const float PHYSICAL_RANGE = 270.0; +const int SERVO_PULSE_MIN_US = 544; +const int SERVO_PULSE_MAX_US = 2400; +const float SERVO_MIN[SERVO_COUNT] = {0.0, 0.0, 90.0, 0.0}; +const float SERVO_MAX[SERVO_COUNT] = {270.0, 112.5, 270.0, 270.0}; + +// Calibrated upright HOME pose shared with the ROS bridge. +float currentAngle[SERVO_COUNT] = {90.0, 35.0, 151.5, 27.5}; +float startAngle[SERVO_COUNT]; +float targetAngle[SERVO_COUNT]; + +Servo servos[SERVO_COUNT]; +bool moving = false; +bool switchWasPressed = false; +unsigned long moveStartedMs = 0; +unsigned long moveDurationMs = 3000; +unsigned long lastServoUpdateMs = 0; +const unsigned long SERVO_UPDATE_INTERVAL_MS = 20; + +char serialLine[128]; +byte serialLength = 0; + +void readSerialCommands(); +void processCommand(char *line); +void updateMotion(); +void updateHoldSwitch(); +void startMove(const float requested[], unsigned long durationMs); +void writeAllOutputs(); +void writePhysicalServo(byte servoIndex, float physicalAngle); +bool anglesAreSafe(const float values[]); + +void setup() +{ + Serial.begin(115200); + pinMode(HOLD_SWITCH_PIN, INPUT_PULLUP); + + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + servos[servo].attach( + SERVO_PINS[servo], SERVO_PULSE_MIN_US, SERVO_PULSE_MAX_US); + } + + writeAllOutputs(); + Serial.println("READY,IK4,1"); +} + +void loop() +{ + readSerialCommands(); + updateHoldSwitch(); + updateMotion(); +} + +void readSerialCommands() +{ + while (Serial.available() > 0) + { + char incoming = Serial.read(); + + if (incoming == '\r') + { + continue; + } + + if (incoming == '\n') + { + serialLine[serialLength] = '\0'; + if (serialLength > 0) + { + processCommand(serialLine); + } + serialLength = 0; + continue; + } + + if (serialLength < sizeof(serialLine) - 1) + { + serialLine[serialLength++] = incoming; + } + else + { + serialLength = 0; + Serial.println("ERROR,line too long"); + } + } +} + +void processCommand(char *line) +{ + if (strcmp(line, "ID") == 0) + { + Serial.println("READY,IK4,1"); + return; + } + + if (strcmp(line, "HOLD") == 0) + { + moving = false; + Serial.println("OK,HOLD"); + return; + } + + char *savePointer; + char *token = strtok_r(line, ",", &savePointer); + + if (token != nullptr && strcmp(token, "JOG") == 0) + { + char *servoToken = strtok_r(nullptr, ",", &savePointer); + char *deltaToken = strtok_r(nullptr, ",", &savePointer); + char *durationToken = strtok_r(nullptr, ",", &savePointer); + if (servoToken == nullptr || deltaToken == nullptr || durationToken == nullptr) + { + Serial.println("ERROR,JOG needs servo,delta,duration"); + return; + } + + int servoNumber = atoi(servoToken); + if (servoNumber < 1 || servoNumber > SERVO_COUNT) + { + Serial.println("ERROR,invalid servo number"); + return; + } + + float requested[SERVO_COUNT]; + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + requested[servo] = currentAngle[servo]; + } + requested[servoNumber - 1] += atof(deltaToken); + + if (!anglesAreSafe(requested)) + { + Serial.println("ERROR,jog outside software limits"); + return; + } + + unsigned long duration = strtoul(durationToken, nullptr, 10); + startMove(requested, constrain(duration, 300UL, 15000UL)); + Serial.println("OK,JOG"); + return; + } + + if (token == nullptr || strcmp(token, "MOVE") != 0) + { + Serial.println("ERROR,unknown command"); + return; + } + + float requested[SERVO_COUNT]; + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + token = strtok_r(nullptr, ",", &savePointer); + if (token == nullptr) + { + Serial.println("ERROR,missing servo angle"); + return; + } + requested[servo] = atof(token); + } + + token = strtok_r(nullptr, ",", &savePointer); + if (token == nullptr) + { + Serial.println("ERROR,missing duration"); + return; + } + + if (!anglesAreSafe(requested)) + { + Serial.println("ERROR,target outside software limits"); + return; + } + + unsigned long duration = strtoul(token, nullptr, 10); + startMove(requested, constrain(duration, 300UL, 15000UL)); + Serial.println("OK,MOVE"); +} + +void startMove(const float requested[], unsigned long durationMs) +{ + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + startAngle[servo] = currentAngle[servo]; + targetAngle[servo] = requested[servo]; + } + moveDurationMs = durationMs; + moveStartedMs = millis(); + lastServoUpdateMs = moveStartedMs - SERVO_UPDATE_INTERVAL_MS; + moving = true; +} + +bool anglesAreSafe(const float values[]) +{ + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + if (values[servo] < SERVO_MIN[servo] || values[servo] > SERVO_MAX[servo]) + { + return false; + } + } + return true; +} + +void updateHoldSwitch() +{ + bool pressed = digitalRead(HOLD_SWITCH_PIN) == LOW; + if (pressed) + { + moving = false; + } + if (pressed && !switchWasPressed) + { + Serial.println("OK,SWITCH HOLD"); + } + switchWasPressed = pressed; +} + +void updateMotion() +{ + if (!moving) + { + return; + } + + unsigned long elapsed = millis() - moveStartedMs; + float progress = constrain( + (float)elapsed / (float)moveDurationMs, 0.0, 1.0); + if (progress < 1.0 && millis() - lastServoUpdateMs < SERVO_UPDATE_INTERVAL_MS) + { + return; + } + lastServoUpdateMs = millis(); + + float progress2 = progress * progress; + float progress3 = progress2 * progress; + float eased = progress3 * (progress * (progress * 6.0 - 15.0) + 10.0); + + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + currentAngle[servo] = startAngle[servo] + (targetAngle[servo] - startAngle[servo]) * eased; + } + writeAllOutputs(); + + if (progress >= 1.0) + { + moving = false; + Serial.println("OK,ARRIVED"); + } +} + +void writeAllOutputs() +{ + for (byte servo = 0; servo < SERVO_COUNT; servo++) + { + writePhysicalServo(servo, currentAngle[servo]); + } +} + +void writePhysicalServo(byte servoIndex, float physicalAngle) +{ + physicalAngle = constrain(physicalAngle, 0.0, PHYSICAL_RANGE); + int pulseUs = round( + SERVO_PULSE_MIN_US + physicalAngle / PHYSICAL_RANGE * (SERVO_PULSE_MAX_US - SERVO_PULSE_MIN_US)); + servos[servoIndex].writeMicroseconds( + constrain(pulseUs, SERVO_PULSE_MIN_US, SERVO_PULSE_MAX_US)); +} diff --git a/waybionic_bringup/CMakeLists.txt b/waybionic_bringup/CMakeLists.txt index fba831a..beae403 100644 --- a/waybionic_bringup/CMakeLists.txt +++ b/waybionic_bringup/CMakeLists.txt @@ -18,6 +18,7 @@ if(BUILD_TESTING) set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() add_launch_test(test/test_ground_station_launch.py) + add_launch_test(test/test_old_arm_launch.py) endif() install(DIRECTORY launch diff --git a/waybionic_bringup/launch/display.launch.py b/waybionic_bringup/launch/display.launch.py index ceab624..7692474 100644 --- a/waybionic_bringup/launch/display.launch.py +++ b/waybionic_bringup/launch/display.launch.py @@ -39,28 +39,27 @@ def generate_launch_description(): waybionic_bringup_dir = get_package_share_directory('waybionic_bringup') default_model_path = os.path.join( - waybionic_desc_dir, 'urdf', 'waybionic_placeholder.urdf' + waybionic_desc_dir, + 'urdf', + 'old_arm_prototype.urdf', ) + default_rviz_config_path = os.path.join( - waybionic_bringup_dir, 'rviz', 'waybionic.rviz' + waybionic_bringup_dir, + 'rviz', + 'waybionic.rviz', ) model_arg = DeclareLaunchArgument( name='model', default_value=default_model_path, - description='Absolute path to robot urdf or xacro file', + description='Absolute path to robot URDF or Xacro file', ) rviz_arg = DeclareLaunchArgument( name='rvizconfig', default_value=default_rviz_config_path, - description='Absolute path to rviz config file', - ) - - joint_state_publisher_gui_node = Node( - package='joint_state_publisher_gui', - executable='joint_state_publisher_gui', - name='joint_state_publisher_gui', + description='Absolute path to RViz config file', ) rviz_node = Node( @@ -71,6 +70,12 @@ def generate_launch_description(): arguments=['-d', LaunchConfiguration('rvizconfig')], ) + joint_state_publisher_gui_node = Node( + package='joint_state_publisher_gui', + executable='joint_state_publisher_gui', + name='joint_state_publisher_gui', + ) + return LaunchDescription([ model_arg, rviz_arg, diff --git a/waybionic_bringup/launch/old_arm.launch.py b/waybionic_bringup/launch/old_arm.launch.py new file mode 100644 index 0000000..1b92227 --- /dev/null +++ b/waybionic_bringup/launch/old_arm.launch.py @@ -0,0 +1,116 @@ +"""Display the handoff's four-servo arm without commanding physical hardware.""" + +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PythonExpression +from launch_ros.actions import Node, PushRosNamespace, SetRemap + + +def generate_launch_description(): + """Reuse the ground station with an isolated model and joint-state topic.""" + description = get_package_share_directory('waybionic_description') + bringup = get_package_share_directory('waybionic_bringup') + arguments = [ + DeclareLaunchArgument( + 'model', + default_value=os.path.join(description, 'urdf', 'waybionic_old_arm.urdf.xacro'), + description='Old-arm Xacro or an expanded, calibrated URDF'), + DeclareLaunchArgument( + 'joint_states_topic', default_value='/old_arm/joint_states', + description='sensor_msgs/JointState input, with named model joints in radians'), + DeclareLaunchArgument( + 'use_joint_state_publisher_gui', default_value='true', + description=( + 'Use the manual joint stepper; set false to use the motion ' + 'test publisher')), + DeclareLaunchArgument( + 'launch_rviz', default_value='true', + description='Set false for headless transform validation'), + DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description='Enable only when an external simulation clock is provided'), + DeclareLaunchArgument( + 'use_mock_diagnostics', default_value='true', + description='Diagnostics remain mock unless a real backend is selected'), + DeclareLaunchArgument( + 'diagnostics_topic', default_value='/diagnostics', + description='DiagnosticArray input for the Engineering Monitor'), + DeclareLaunchArgument( + 'start_motion_test', default_value='true', + description='Start the old-arm motion test node'), + DeclareLaunchArgument( + 'motion_test_segment_duration', default_value='3.0', + description='Seconds for each synchronized motion segment'), + DeclareLaunchArgument( + 'hardware_mode', default_value='simulation', + description='Arm mode: simulation or arduino'), + DeclareLaunchArgument( + 'arduino_port', default_value='', + description='Arduino serial port, for example /dev/ttyACM0'), + DeclareLaunchArgument( + 'arduino_baud', default_value='115200', + description='Arduino serial baud rate'), + DeclareLaunchArgument( + 'arduino_dry_run', default_value='false', + description='Run the Arduino bridge without opening a serial port'), + ] + station = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup, 'launch', 'ground_station.launch.py')), + launch_arguments={ + 'model': LaunchConfiguration('model'), + 'rvizconfig': os.path.join(bringup, 'rviz', 'waybionic_old_arm.rviz'), + 'use_joint_state_publisher_gui': LaunchConfiguration('use_joint_state_publisher_gui'), + 'launch_rviz': LaunchConfiguration('launch_rviz'), + 'use_sim_time': LaunchConfiguration('use_sim_time'), + 'use_mock_diagnostics': LaunchConfiguration('use_mock_diagnostics'), + 'diagnostics_topic': LaunchConfiguration('diagnostics_topic'), + 'start_temporary_diagnostics_publisher': 'false', + }.items(), + ) + + motion_test_node = Node( + package='waybionic_motion_test', + executable='motion_test', + name='old_arm_motion_test', + output='screen', + condition=IfCondition(PythonExpression([ + "'", LaunchConfiguration('start_motion_test'), "' == 'true' and '", + LaunchConfiguration('hardware_mode'), "' == 'simulation' and '", + LaunchConfiguration('use_joint_state_publisher_gui'), "' != 'true'" + ])), + parameters=[ + {'segment_duration': LaunchConfiguration( + 'motion_test_segment_duration')} + ], + ) + + arduino_bridge_node = Node( + package='waybionic_hardware', + executable='arduino_bridge', + name='waybionic_arduino_bridge', + output='screen', + condition=IfCondition(PythonExpression([ + "'", LaunchConfiguration('hardware_mode'), "' == 'arduino'" + ])), + parameters=[ + {'port': LaunchConfiguration('arduino_port')}, + {'baud': LaunchConfiguration('arduino_baud')}, + {'segment_duration': LaunchConfiguration( + 'motion_test_segment_duration')}, + {'dry_run': LaunchConfiguration('arduino_dry_run')}, + ], + ) + + return LaunchDescription(arguments + [GroupAction([ + PushRosNamespace('old_arm'), + SetRemap(src='joint_states', dst=LaunchConfiguration('joint_states_topic')), + station, + motion_test_node, + arduino_bridge_node, + ])]) diff --git a/waybionic_bringup/package.xml b/waybionic_bringup/package.xml index d35d763..c07973f 100644 --- a/waybionic_bringup/package.xml +++ b/waybionic_bringup/package.xml @@ -12,7 +12,9 @@ ament_index_python xacro waybionic_description + waybionic_hardware waybionic_rviz_plugins + waybionic_motion_test waybionic_control launch launch_ros diff --git a/waybionic_bringup/rviz/waybionic.rviz b/waybionic_bringup/rviz/waybionic.rviz index 6c1f353..d5b5d94 100644 --- a/waybionic_bringup/rviz/waybionic.rviz +++ b/waybionic_bringup/rviz/waybionic.rviz @@ -1,21 +1,23 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 0 Name: Displays Property Tree Widget: Expanded: - /Global Options1 - /Status1 - /RobotModel1 + - /RobotModel1/Mass Properties1 - /RobotModel1/Description Topic1 Splitter Ratio: 0.5 - Tree Height: 555 + Tree Height: 971 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties Expanded: - /2D Goal Pose1 - /Publish Point1 + - /Interact1 Name: Tool Properties Splitter Ratio: 0.5886790156364441 - Class: rviz_common/Views @@ -28,6 +30,14 @@ Panels: Name: Time SyncMode: 0 SyncSource: "" + - Class: waybionic_rviz_plugins/MotionTestPanel + Name: MotionTestPanel + - Class: waybionic_rviz_plugins/DiagnosticsPanel + Diagnostics Topic: /diagnostics + Name: DiagnosticsPanel + Use Mock Diagnostics: true + - Class: rviz_common/Help + Name: Help Visualization Manager: Class: "" Displays: @@ -52,14 +62,14 @@ Visualization Manager: - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false - Description File: "" - Description Source: Topic + Description File: /home/khidri/waybionic_ground_station/install/waybionic_description/share/waybionic_description/urdf/waybionic_placeholder.urdf + Description Source: File Description Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable - Value: /robot_description + Value: "" Enabled: true Links: All Links Enabled: true @@ -85,28 +95,6 @@ Visualization Manager: Update Interval: 0 Value: true Visual Enabled: true - - Class: rviz_default_plugins/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: true - arm_link: - Value: true - base_link: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: false - Tree: - base_link: - arm_link: - {} - Update Interval: 0 - Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -114,11 +102,6 @@ Visualization Manager: Frame Rate: 30 Name: root Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose @@ -146,6 +129,8 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /clicked_point + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true Transformation: Current: Class: rviz_default_plugins/TF @@ -153,7 +138,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.5597326755523682 + Distance: 2.116607427597046 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -163,23 +148,169 @@ Visualization Manager: X: 0 Y: 0 Z: 0 - Focal Shape Fixed Size: true + Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.7853981852531433 Target Frame: - Value: Orbit (rviz) - Yaw: 0.785398006439209 - Saved: ~ + Value: Orbit (rviz_default_plugins) + Yaw: 0.7853981852531433 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 + - Class: rviz_default_plugins/Orbit + Distance: 8.34925651550293 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.009999999776482582 + Pitch: 0.38979676365852356 + Target Frame: + Value: Orbit (rviz) + Yaw: 4.518586158752441 Window Geometry: + DiagnosticsPanel: + collapsed: false Displays: collapsed: false - Height: 846 + Height: 1612 + Help: + collapsed: false Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionTestPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000200000000000001a4000005f6fc0200000004fb0000001200530065006c0065006300740069006f006e00000001080000005c0000005c00fffffffb0000000800540069006d00650000000247000000370000003700fffffffb000000200044006900610067006e006f0073007400690063007300500061006e0065006c010000003b000005f60000035100fffffffb0000000800480065006c007000000003ca0000006e0000006e00ffffff0000000100000168000005f6fc0200000004fb0000001e004d006f00740069006f006e005400650073007400500061006e0065006c010000003b000000d2000000cc00fffffffb0000000a005600690065007700730100000113000000b0000000a000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007301000001c90000005c0000005c00fffffffb000000100044006900730070006c006100790073010000022b00000406000000c700ffffff0000045c000005f600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -188,6 +319,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 1200 - X: 3639 - Y: 407 + Width: 1908 + X: 0 + Y: 0 diff --git a/waybionic_bringup/rviz/waybionic_old_arm.rviz b/waybionic_bringup/rviz/waybionic_old_arm.rviz new file mode 100644 index 0000000..625879f --- /dev/null +++ b/waybionic_bringup/rviz/waybionic_old_arm.rviz @@ -0,0 +1,249 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 544 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: waybionic_rviz_plugins/DiagnosticsPanel + Diagnostics Topic: /diagnostics + Name: Engineering Monitor + Use Mock Diagnostics: true + - Class: waybionic_rviz_plugins/MotionTestPanel + Name: MotionTestPanel +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.44999998807907104 + Cell Size: 0.05000000074505806 + Class: rviz_default_plugins/Grid + Color: 155; 155; 155 + Enabled: true + Line Style: + Line Width: 0.0010000000474974513 + Value: Lines + Name: Grid (50 mm) + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: -0.06599999964237213 + Plane: XY + Plane Cell Count: 12 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /old_arm/robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + old_arm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_base_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_base_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_left_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_left_servo: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_right_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_right_servo: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_cover_screws: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_horn: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_horn: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + old_arm_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_marker: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_marker_cross: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_roll: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: Old Arm (Video Reference Model) + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.03500000014901161 + Name: Wrist Target Frame + Radius: 0.001500000013038516 + Reference Frame: old_arm_tool_frame + Value: true + Enabled: true + Global Options: + Background Color: 238; 240; 242 + Fixed Frame: old_arm_base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.7799999713897705 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.07999999821186066 + Y: 0 + Z: 0.12999999523162842 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.004999999888241291 + Pitch: 0.44999998807907104 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 0.800000011920929 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Engineering Monitor: + collapsed: false + Height: 1143 + Hide Left Dock: false + Hide Right Dock: false + MotionTestPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000200000000000001a400000421fc0200000001fb000000260045006e00670069006e0065006500720069006e00670020004d006f006e00690074006f0072010000003b000004210000034f00ffffff000000010000016800000421fc0200000003fb0000001e004d006f00740069006f006e005400650073007400500061006e0065006c010000003b000000d4000000cc00fffffffb000000100044006900730070006c0061007900730100000115000002a1000000c700fffffffb0000000a0056006900650077007301000003bc000000a0000000a000ffffff0000045c0000042100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1908 + X: 0 + Y: 0 diff --git a/waybionic_bringup/rviz/waybionic_old_arm_root.rviz b/waybionic_bringup/rviz/waybionic_old_arm_root.rviz new file mode 100644 index 0000000..fb94d44 --- /dev/null +++ b/waybionic_bringup/rviz/waybionic_old_arm_root.rviz @@ -0,0 +1,269 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 70 + Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 451 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: waybionic_rviz_plugins/MotionTestPanel + Name: MotionTestPanel + - Class: waybionic_rviz_plugins/DiagnosticsPanel + Diagnostics Topic: /diagnostics + Name: DiagnosticsPanel + Use Mock Diagnostics: true +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.44999998807907104 + Cell Size: 0.05000000074505806 + Class: rviz_default_plugins/Grid + Color: 155; 155; 155 + Enabled: true + Line Style: + Line Width: 0.0010000000474974513 + Value: Lines + Name: Grid (50 mm) + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: -0.06599999964237213 + Plane: XY + Plane Cell Count: 12 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + old_arm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_base_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_base_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_left_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_left_servo: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_right_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_distal_right_servo: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_cover_screws: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_horn: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_elbow_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_forearm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_horn: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_mount: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_shoulder_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + old_arm_upper_arm: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_hub: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_marker: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_marker_cross: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_roll: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_servo_body: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + old_arm_wrist_servo_cover: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: Old Arm (Video Reference Model) + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.03500000014901161 + Name: Wrist Target Frame + Radius: 0.001500000013038516 + Reference Frame: old_arm_tool_frame + Value: true + Enabled: true + Global Options: + Background Color: 238; 240; 242 + Fixed Frame: old_arm_base_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 0.7799999713897705 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.07999999821186066 + Y: 0 + Z: 0.12999999523162842 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.004999999888241291 + Pitch: 0.024999983608722687 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 2.640000581741333 + Saved: + - Class: rviz_default_plugins/Orbit + Distance: 0.7799999713897705 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.07999999821186066 + Y: 0 + Z: 0.12999999523162842 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Orbit + Near Clip Distance: 0.004999999888241291 + Pitch: 0.024999983608722687 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 2.640000581741333 +Window Geometry: + DiagnosticsPanel: + collapsed: false + Displays: + collapsed: false + Height: 1143 + Hide Left Dock: false + Hide Right Dock: false + MotionTestPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000200000000000001a400000421fc0200000002fb0000001e004d006f00740069006f006e005400650073007400500061006e0065006c010000003b000000cc000000cc00fffffffb000000200044006900610067006e006f0073007400690063007300500061006e0065006c010000010d0000034f0000034f00ffffff000000010000015600000421fc0200000002fb000000100044006900730070006c006100790073010000003b00000244000000c700fffffffb0000000a005600690065007700730100000285000001d7000000a000ffffff0000046e0000042100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false + Width: 1908 + X: 0 + Y: 0 diff --git a/waybionic_bringup/rviz/waybionic_unified.rviz b/waybionic_bringup/rviz/waybionic_unified.rviz index 135994e..a127180 100644 --- a/waybionic_bringup/rviz/waybionic_unified.rviz +++ b/waybionic_bringup/rviz/waybionic_unified.rviz @@ -32,6 +32,8 @@ Panels: Diagnostics Topic: /diagnostics Name: DiagnosticsPanel Use Mock Diagnostics: true + - Class: waybionic_rviz_plugins/MotionTestPanel + Name: MotionTestPanel Visualization Manager: Class: "" Displays: @@ -71,12 +73,32 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - arm_link: + base_link: Alpha: 1 Show Axes: false Show Trail: false Value: true - base_link: + base_rotation_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + end_effector: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + upper_arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + wrist_link: Alpha: 1 Show Axes: false Show Trail: false @@ -96,10 +118,18 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true - arm_link: - Value: true base_link: Value: true + base_rotation_link: + Value: true + end_effector: + Value: true + forearm_link: + Value: true + upper_arm_link: + Value: true + wrist_link: + Value: true Marker Scale: 1 Name: TF Show Arrows: true @@ -107,8 +137,12 @@ Visualization Manager: Show Names: false Tree: base_link: - arm_link: - {} + base_rotation_link: + upper_arm_link: + forearm_link: + wrist_link: + end_effector: + {} Update Interval: 0 Value: true Enabled: true @@ -172,20 +206,22 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.785398006439209 + Pitch: 0.3753979504108429 Target Frame: Value: Orbit (rviz) - Yaw: 0.785398006439209 + Yaw: 1.030397653579712 Saved: ~ Window Geometry: DiagnosticsPanel: collapsed: false Displays: collapsed: false - Height: 2055 + Height: 1243 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000400000000000001a400000774fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000200044006900610067006e006f0073007400690063007300500061006e0065006c0100000108000002330000023400fffffffb000000200044006900610067006e006f0073007400690063007300500061006e0065006c0100000341000002340000023400fffffffb000000200044006900610067006e006f0073007400690063007300500061006e0065006c010000057b000002340000023400ffffff000000010000010f00000774fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000774000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000002ea00000037fc0100000002fb0000000800540069006d00650100000000000002ea0000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000002b0000077500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionTestPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd00000004000000000000024700000448fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000000c7000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000200044006900610067006e006f0073007400690063007300500061006e0065006c01000001080000037b0000035100fffffffb000000200044006900610067006e006f0073007400690063007300500061006e0065006c0100000341000002340000000000000000fb000000200044006900610067006e006f0073007400690063007300500061006e0065006c010000057b000002340000000000000000000000010000016800000448fc0200000004fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000001e004d006f00740069006f006e005400650073007400500061006e0065006c010000003b000000dc000000cc00fffffffb0000000a00560069006500770073010000011d00000366000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077400000037fc0100000002fb0000000800540069006d00650100000000000007740000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000003b90000044800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -194,6 +230,6 @@ Window Geometry: collapsed: false Views: collapsed: false - Width: 746 - X: 57 - Y: 4 + Width: 1908 + X: -29 + Y: -33 diff --git a/waybionic_bringup/test/test_ground_station_launch.py b/waybionic_bringup/test/test_ground_station_launch.py index 3476a7b..16d9671 100644 --- a/waybionic_bringup/test/test_ground_station_launch.py +++ b/waybionic_bringup/test/test_ground_station_launch.py @@ -4,7 +4,7 @@ from ament_index_python.packages import get_package_share_directory import launch -from launch.actions import IncludeLaunchDescription +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable from launch.launch_description_sources import PythonLaunchDescriptionSource import launch_testing import launch_testing.actions @@ -26,6 +26,7 @@ def generate_test_description(): ) return launch.LaunchDescription([ + SetEnvironmentVariable('ROS_DOMAIN_ID', '71'), ground_station_launch, launch_testing.actions.ReadyToTest() ]) diff --git a/waybionic_bringup/test/test_old_arm_launch.py b/waybionic_bringup/test/test_old_arm_launch.py new file mode 100644 index 0000000..3bd5fab --- /dev/null +++ b/waybionic_bringup/test/test_old_arm_launch.py @@ -0,0 +1,220 @@ +"""Check the handoff model and its live JointState-to-TF path without hardware.""" + +import math +import os +from pathlib import Path +import struct +import subprocess +import time +import unittest +import uuid +import xml.etree.ElementTree as ET + +from ament_index_python.packages import get_package_share_directory +import launch +from launch.actions import IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +import launch_testing +import launch_testing.actions +import pytest +import rclpy +from rclpy.time import Time +from sensor_msgs.msg import JointState +from tf2_ros import Buffer, TransformListener + + +JOINT_NAMES = [ + 'old_arm_base_yaw_joint', + 'old_arm_shoulder_pitch_joint', + 'old_arm_elbow_pitch_joint', + 'old_arm_wrist_roll_joint', +] + + +def expand_model(*mappings): + """Expand the installed model, including any calibration overrides.""" + source = os.path.join( + get_package_share_directory('waybionic_description'), + 'urdf', 'waybionic_old_arm.urdf.xacro') + result = subprocess.run( + ['xacro', source, *mappings], check=True, capture_output=True, text=True) + return ET.fromstring(result.stdout) + + +def expected_wrist_position(positions): + """Evaluate the handoff's independent forward-kinematics equations.""" + base, shoulder, elbow, _ = positions + radial = 0.025 + 0.120 * math.cos(shoulder) + 0.050 * math.cos(shoulder + elbow) + height = 0.070 + 0.120 * math.sin(shoulder) + 0.050 * math.sin(shoulder + elbow) + return radial * math.cos(base), radial * math.sin(base), height + + +@pytest.mark.launch_test +def generate_test_description(): + """Launch only the old-arm state publisher on a unique input topic.""" + bringup = get_package_share_directory('waybionic_bringup') + topic = '/old_arm_test_' + uuid.uuid4().hex + '/joint_states' + station = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(bringup, 'launch', 'old_arm.launch.py')), + launch_arguments={ + 'launch_rviz': 'false', + 'use_joint_state_publisher_gui': 'false', + 'joint_states_topic': topic, + 'start_motion_test': 'false', + }.items(), + ) + return launch.LaunchDescription([ + SetEnvironmentVariable('ROS_DOMAIN_ID', '72'), + station, launch_testing.actions.ReadyToTest(), + ]), {'joint_states_topic': topic} + + +class TestOldArmDescription(unittest.TestCase): + """Protect handoff calibration and visualization-only model boundaries.""" + + def test_joint_contract_and_limits(self): + """Keep four named joints with handoff software ranges in radians.""" + robot = expand_model() + joints = [joint for joint in robot.findall('joint') if joint.get('type') != 'fixed'] + self.assertEqual([joint.get('name') for joint in joints], JOINT_NAMES) + expected_ranges = [(-90.0, 180.0), (12.5, 125.0), (-61.5, 118.5), (-27.5, 242.5)] + for joint, expected in zip(joints, expected_ranges): + self.assertEqual(joint.get('type'), 'revolute') + limits = joint.find('limit') + for key, degrees in zip(('lower', 'upper'), expected): + self.assertAlmostEqual(float(limits.get(key)), math.radians(degrees)) + self.assertEqual(float(limits.get('effort')), 0.0) + self.assertEqual(float(limits.get('velocity')), 0.0) + self.assertEqual(joints[0].find('axis').get('xyz'), '0 0 1') + self.assertEqual(joints[1].find('axis').get('xyz'), '0 -1 0') + self.assertEqual(joints[2].find('axis').get('xyz'), '0 -1 0') + self.assertEqual(joints[3].find('axis').get('xyz'), '1 0 0') + self.assertFalse(robot.findall('.//inertial')) + self.assertFalse(robot.findall('.//collision')) + self.assertFalse(robot.findall('.//transmission')) + self.assertFalse(robot.findall('.//ros2_control')) + for link in robot.findall('link'): + materials = {visual.find('material').get('name') for visual in link.findall('visual')} + self.assertLessEqual(len(materials), 1, 'RViz requires separate color groups per link') + + def test_geometry_and_calibration_overrides(self): + """Allow remeasured lengths and direction signs without changing meshes.""" + robot = expand_model('upper_arm_length:=0.130', 'base_direction:=-1') + joints = {joint.get('name'): joint for joint in robot.findall('joint')} + self.assertEqual( + [float(value) for value in + joints['old_arm_elbow_pitch_joint'].find('origin').get('xyz').split()], + [0.130, 0.0, 0.0]) + limits = joints['old_arm_base_yaw_joint'].find('limit') + self.assertAlmostEqual(float(limits.get('lower')), math.radians(-180.0)) + self.assertAlmostEqual(float(limits.get('upper')), math.radians(90.0)) + for box in robot.findall('.//box'): + self.assertTrue(all(float(value) > 0 for value in box.get('size').split())) + for cylinder in robot.findall('.//cylinder'): + self.assertGreater(float(cylinder.get('radius')), 0) + self.assertGreater(float(cylinder.get('length')), 0) + upper_meshes = robot.findall("link[@name='old_arm_upper_arm']/visual/geometry/mesh") + self.assertEqual(len(upper_meshes), 2) + for mesh in upper_meshes: + self.assertAlmostEqual(float(mesh.get('scale').split()[0]), 0.130 / 0.120) + + def test_installed_video_meshes(self): + """Require the six finite, meter-scale STL resources in the installed package.""" + robot = expand_model() + directory = Path(get_package_share_directory('waybionic_description')) + filenames = set() + for mesh in robot.findall('.//mesh'): + filename = mesh.get('filename') + prefix = 'package://waybionic_description/' + self.assertTrue(filename.startswith(prefix)) + target = directory / filename.removeprefix(prefix) + content = target.read_bytes() + triangle_count = struct.unpack_from(' 0 for value in mesh.get('scale').split())) + filenames.add(target.name) + self.assertEqual(filenames, { + 'base_plate.stl', 'shoulder_motor_plate.stl', 'shoulder_back_plate.stl', + 'upper_arm_plate.stl', 'wrist_housing.stl', 'wrist_cage.stl', + }) + + def test_distal_servos_remain_visual_only(self): + """Represent the additional wrist hardware without inventing control joints.""" + robot = expand_model() + for name in ('old_arm_distal_left_servo_mount', 'old_arm_distal_right_servo_mount'): + joint = robot.find(f"joint[@name='{name}']") + self.assertIsNotNone(joint) + self.assertEqual(joint.get('type'), 'fixed') + self.assertEqual(joint.find('parent').get('link'), 'old_arm_wrist_roll') + + +class TestOldArmTransforms(unittest.TestCase): + """Verify live input reaches the intended model and moves its wrist frame.""" + + def test_joint_states_drive_calibrated_transforms(self, joint_states_topic): + """Match zero, yaw, shoulder, elbow, combined and roll poses to the handoff.""" + rclpy.init() + node = rclpy.create_node('old_arm_transform_test_' + uuid.uuid4().hex) + buffer = Buffer() + listener = TransformListener(buffer, node) + publisher = node.create_publisher(JointState, joint_states_topic, 10) + try: + poses = [ + (0.0, 0.0, 0.0, 0.0), + (math.pi / 2, 0.0, 0.0, 0.0), + (0.0, math.pi / 2, 0.0, 0.0), + (0.0, 0.0, math.pi / 3, 0.0), + (math.pi / 4, math.pi / 4, -math.pi / 4, 0.0), + (0.0, 0.0, 0.0, math.pi / 2), + ] + for positions in poses: + expected = expected_wrist_position(positions) + first_stamp = node.get_clock().now().nanoseconds + deadline = time.monotonic() + 8.0 + matched = False + while time.monotonic() < deadline: + message = JointState() + message.header.stamp = node.get_clock().now().to_msg() + message.name = JOINT_NAMES + message.position = list(positions) + publisher.publish(message) + rclpy.spin_once(node, timeout_sec=0.05) + if not buffer.can_transform('old_arm_base_link', 'old_arm_tool_frame', Time()): + continue + transform = buffer.lookup_transform( + 'old_arm_base_link', 'old_arm_tool_frame', Time()) + stamp = transform.header.stamp + if stamp.sec * 1000000000 + stamp.nanosec < first_stamp: + continue + translation = transform.transform.translation + actual = (translation.x, translation.y, translation.z) + if not all(math.isclose(value, target, abs_tol=1e-6) + for value, target in zip(actual, expected)): + continue + if positions[3] != 0.0: + rotation = transform.transform.rotation + aligned = abs((rotation.x + rotation.w) / math.sqrt(2)) + if not math.isclose(aligned, 1.0, abs_tol=1e-6): + continue + matched = True + break + self.assertTrue(matched, f'No matching current transform for pose {positions}') + self.assertEqual(node.count_publishers(joint_states_topic), 1) + self.assertGreater(publisher.get_subscription_count(), 0) + finally: + listener.unregister() + node.destroy_node() + rclpy.shutdown() + + +@launch_testing.post_shutdown_test() +class TestOldArmShutdown(unittest.TestCase): + """Require a clean state-publisher shutdown.""" + + def test_exit_codes(self, proc_info): + """Reject unexpected child process failures.""" + launch_testing.asserts.assertExitCodes(proc_info, allowable_exit_codes=[0, -2]) diff --git a/waybionic_description/meshes/old_arm/base_plate.stl b/waybionic_description/meshes/old_arm/base_plate.stl new file mode 100644 index 0000000..6443ee7 Binary files /dev/null and b/waybionic_description/meshes/old_arm/base_plate.stl differ diff --git a/waybionic_description/meshes/old_arm/shoulder_back_plate.stl b/waybionic_description/meshes/old_arm/shoulder_back_plate.stl new file mode 100644 index 0000000..4a4ce15 Binary files /dev/null and b/waybionic_description/meshes/old_arm/shoulder_back_plate.stl differ diff --git a/waybionic_description/meshes/old_arm/shoulder_motor_plate.stl b/waybionic_description/meshes/old_arm/shoulder_motor_plate.stl new file mode 100644 index 0000000..3b724aa Binary files /dev/null and b/waybionic_description/meshes/old_arm/shoulder_motor_plate.stl differ diff --git a/waybionic_description/meshes/old_arm/upper_arm_plate.stl b/waybionic_description/meshes/old_arm/upper_arm_plate.stl new file mode 100644 index 0000000..687b5c6 Binary files /dev/null and b/waybionic_description/meshes/old_arm/upper_arm_plate.stl differ diff --git a/waybionic_description/meshes/old_arm/wrist_cage.stl b/waybionic_description/meshes/old_arm/wrist_cage.stl new file mode 100644 index 0000000..df3062f Binary files /dev/null and b/waybionic_description/meshes/old_arm/wrist_cage.stl differ diff --git a/waybionic_description/meshes/old_arm/wrist_housing.stl b/waybionic_description/meshes/old_arm/wrist_housing.stl new file mode 100644 index 0000000..ce94f85 Binary files /dev/null and b/waybionic_description/meshes/old_arm/wrist_housing.stl differ diff --git a/waybionic_description/urdf/old_arm_prototype.urdf b/waybionic_description/urdf/old_arm_prototype.urdf new file mode 100644 index 0000000..587c34a --- /dev/null +++ b/waybionic_description/urdf/old_arm_prototype.urdf @@ -0,0 +1,103 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/waybionic_description/urdf/waybionic_old_arm.expanded.urdf b/waybionic_description/urdf/waybionic_old_arm.expanded.urdf new file mode 100644 index 0000000..d82af3d --- /dev/null +++ b/waybionic_description/urdf/waybionic_old_arm.expanded.urdf @@ -0,0 +1,518 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/waybionic_description/urdf/waybionic_old_arm.urdf.xacro b/waybionic_description/urdf/waybionic_old_arm.urdf.xacro new file mode 100644 index 0000000..83459ec --- /dev/null +++ b/waybionic_description/urdf/waybionic_old_arm.urdf.xacro @@ -0,0 +1,255 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/waybionic_hardware/package.xml b/waybionic_hardware/package.xml new file mode 100644 index 0000000..7108013 --- /dev/null +++ b/waybionic_hardware/package.xml @@ -0,0 +1,23 @@ + + + waybionic_hardware + 0.1.0 + Arduino serial bridge for the WayBionic four-servo arm. + + WayBionic + Apache-2.0 + + diagnostic_msgs + rclpy + sensor_msgs + std_msgs + python3-serial + + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/waybionic_hardware/resource/waybionic_hardware b/waybionic_hardware/resource/waybionic_hardware new file mode 100644 index 0000000..289bec5 --- /dev/null +++ b/waybionic_hardware/resource/waybionic_hardware @@ -0,0 +1 @@ +# ament package marker \ No newline at end of file diff --git a/waybionic_hardware/setup.cfg b/waybionic_hardware/setup.cfg new file mode 100644 index 0000000..1b8cd9c --- /dev/null +++ b/waybionic_hardware/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/waybionic_hardware + +[install] +install_scripts=$base/lib/waybionic_hardware \ No newline at end of file diff --git a/waybionic_hardware/setup.py b/waybionic_hardware/setup.py new file mode 100644 index 0000000..576bd2b --- /dev/null +++ b/waybionic_hardware/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup + + +package_name = 'waybionic_hardware' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools', 'pyserial'], + zip_safe=True, + description='Arduino serial bridge for the WayBionic four-servo arm.', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'arduino_bridge = waybionic_hardware.arduino_bridge:main', + ], + }, +) \ No newline at end of file diff --git a/waybionic_hardware/test/__init__.py b/waybionic_hardware/test/__init__.py new file mode 100644 index 0000000..fe7dbee --- /dev/null +++ b/waybionic_hardware/test/__init__.py @@ -0,0 +1 @@ +# Test module for waybionic_hardware diff --git a/waybionic_hardware/test/test_arduino_bridge.py b/waybionic_hardware/test/test_arduino_bridge.py new file mode 100644 index 0000000..b858371 --- /dev/null +++ b/waybionic_hardware/test/test_arduino_bridge.py @@ -0,0 +1,216 @@ +"""Minimal tests for Arduino bridge module.""" +import unittest +from types import SimpleNamespace + + +class TestArduinoBridge(unittest.TestCase): + """Test cases for ArduinoBridge.""" + + def test_import(self): + """Test that the module can be imported.""" + try: + from waybionic_hardware import arduino_bridge # noqa: F401 + self.assertTrue(True) + except ImportError: + self.fail("Failed to import arduino_bridge") + + def test_joint_names_match_old_arm_model(self): + """Joint states target the imported old-arm URDF joints.""" + from waybionic_hardware.arduino_bridge import JOINT_NAMES + + self.assertEqual(JOINT_NAMES, [ + 'old_arm_base_yaw_joint', + 'old_arm_shoulder_pitch_joint', + 'old_arm_elbow_pitch_joint', + 'old_arm_wrist_roll_joint', + ]) + + def make_bridge(self): + from waybionic_hardware.arduino_bridge import ArduinoBridge + + bridge = ArduinoBridge.__new__(ArduinoBridge) + bridge.serial_port = SimpleNamespace(writes=[]) + bridge.serial_port.write = lambda data: bridge.serial_port.writes.append(data) + bridge.serial_lock = __import__('threading').Lock() + bridge.state_lock = __import__('threading').RLock() + bridge.dry_run = False + bridge.connected = True + bridge.ready = True + bridge.faulted = False + bridge.motion_active = True + bridge.motion_duration = 0.0 + bridge.sequence_active = True + bridge.command_kind = 'RUN' + bridge.stop_requested = False + bridge.home_completed = True + bridge.awaiting_probe = False + bridge.probe_sent_monotonic = None + bridge.last_error = '' + bridge.last_status = 'moving' + bridge.get_logger = lambda: SimpleNamespace(debug=lambda message: None) + return bridge + + def test_error_latches_fault_and_stops_motion(self): + bridge = self.make_bridge() + + bridge.process_serial_line('ERROR,OVER_CURRENT') + + self.assertFalse(bridge.connected) + self.assertFalse(bridge.ready) + self.assertTrue(bridge.faulted) + self.assertFalse(bridge.motion_active) + self.assertFalse(bridge.sequence_active) + self.assertEqual(bridge.serial_port.writes, [b'HOLD\n']) + + def test_malformed_response_latches_fault_and_stops_motion(self): + bridge = self.make_bridge() + + bridge.process_serial_line('garbled response') + + self.assertEqual(bridge.last_status, 'malformed-response') + self.assertTrue(bridge.faulted) + self.assertFalse(bridge.motion_active) + self.assertEqual(bridge.serial_port.writes, [b'HOLD\n']) + + def test_read_failure_freezes_motion_and_requests_hold(self): + bridge = self.make_bridge() + + bridge._latch_fault('serial-read-failed', 'read failed') + + self.assertFalse(bridge.connected) + self.assertFalse(bridge.motion_active) + self.assertFalse(bridge.sequence_active) + self.assertEqual(bridge.serial_port.writes, [b'HOLD\n']) + + def test_response_watchdog_latches_fault(self): + from waybionic_hardware.arduino_bridge import RESPONSE_TIMEOUT_SECONDS + + bridge = self.make_bridge() + bridge.last_response_monotonic = __import__('time').monotonic() - ( + RESPONSE_TIMEOUT_SECONDS + 1.0) + bridge.current = [0.0, 0.0, 0.0, 0.0] + bridge.get_clock = lambda: SimpleNamespace( + now=lambda: SimpleNamespace(to_msg=lambda: None)) + bridge.joint_publisher = SimpleNamespace(publish=lambda message: None) + + bridge.publish_estimated_state() + + self.assertTrue(bridge.faulted) + self.assertEqual(bridge.last_status, 'serial-read-failed') + self.assertEqual(bridge.serial_port.writes, [b'HOLD\n']) + + def test_arrived_after_stop_does_not_start_next_move(self): + from std_msgs.msg import String + from waybionic_hardware.arduino_bridge import ArduinoBridge + + bridge = self.make_bridge() + bridge.send_move = lambda target, command_kind: bridge.serial_port.writes.append( + f'{command_kind}:{target}') + bridge.handle_command(String(data='STOP')) + bridge.process_serial_line('OK,ARRIVED') + + self.assertTrue(bridge.stop_requested) + self.assertFalse(bridge.sequence_active) + self.assertEqual(len(bridge.serial_port.writes), 1) + self.assertEqual(bridge.serial_port.writes[0], b'HOLD\n') + + def test_dry_run_stop_completes_hold(self): + from std_msgs.msg import String + + bridge = self.make_bridge() + bridge.dry_run = True + + bridge.handle_command(String(data='STOP')) + + self.assertEqual(bridge.last_status, 'held') + self.assertTrue(bridge.stop_requested) + self.assertFalse(bridge.motion_active) + + def test_home_and_run_require_ready_home_sequence(self): + from std_msgs.msg import String + + bridge = self.make_bridge() + bridge.ready = False + bridge.handle_command(String(data='HOME')) + self.assertEqual(bridge.last_status, 'home-rejected-not-ready') + + bridge.ready = True + bridge.home_completed = False + bridge.handle_command(String(data='RUN')) + self.assertEqual(bridge.last_status, 'run-rejected-home-required') + + def test_fault_is_reported_as_diagnostic_error(self): + from diagnostic_msgs.msg import DiagnosticStatus + + bridge = self.make_bridge() + bridge.faulted = True + bridge.last_status = 'arduino-error' + bridge.diagnostics_publisher = SimpleNamespace(publish=lambda message: setattr(bridge, 'diagnostic', message)) + bridge.status_publisher = SimpleNamespace(publish=lambda message: setattr(bridge, 'motion_status', message)) + bridge.count_subscribers = lambda topic: 1 + bridge.port = '/dev/fake' + bridge.baud = 115200 + bridge.get_clock = lambda: SimpleNamespace( + now=lambda: SimpleNamespace(to_msg=lambda: None)) + + bridge.publish_diagnostics() + + self.assertEqual(bridge.diagnostic.status[0].level, DiagnosticStatus.ERROR) + self.assertEqual(bridge.diagnostic.status[1].message, 'robot_state_publisher connected') + self.assertEqual(bridge.diagnostic.status[2].level, DiagnosticStatus.WARN) + + def test_unhandshaken_connection_is_not_reported_healthy(self): + from diagnostic_msgs.msg import DiagnosticStatus + + bridge = self.make_bridge() + bridge.ready = False + bridge.last_status = 'connected' + bridge.diagnostics_publisher = SimpleNamespace(publish=lambda message: setattr(bridge, 'diagnostic', message)) + bridge.status_publisher = SimpleNamespace(publish=lambda message: setattr(bridge, 'motion_status', message)) + bridge.count_subscribers = lambda topic: 1 + bridge.port = '/dev/fake' + bridge.baud = 115200 + bridge.get_clock = lambda: SimpleNamespace( + now=lambda: SimpleNamespace(to_msg=lambda: None)) + + bridge.publish_diagnostics() + + self.assertEqual(bridge.diagnostic.status[0].level, DiagnosticStatus.WARN) + self.assertEqual(bridge.motion_status.data, 'CONNECTING') + + def test_shutdown_requests_hold_before_closing_port(self): + import rclpy + from unittest.mock import patch + from waybionic_hardware.arduino_bridge import ArduinoBridge + + class FakePort: + def __init__(self): + self.writes = [] + self.closed = False + + def write(self, data): + self.writes.append(data) + + def close(self): + self.closed = True + + rclpy.init() + try: + with patch.object(ArduinoBridge, 'connect_to_arduino'): + bridge = ArduinoBridge() + bridge.serial_port = FakePort() + bridge.connected = True + bridge.motion_active = True + bridge.serial_reader = None + + bridge.destroy_node() + + self.assertEqual(bridge.serial_port.writes, [b'HOLD\n']) + self.assertTrue(bridge.serial_port.closed) + finally: + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + unittest.main() diff --git a/waybionic_hardware/test/test_smoke.py b/waybionic_hardware/test/test_smoke.py new file mode 100644 index 0000000..9993f44 --- /dev/null +++ b/waybionic_hardware/test/test_smoke.py @@ -0,0 +1,2 @@ +def test_smoke_passes(): + assert True diff --git a/waybionic_hardware/waybionic_hardware/__init__.py b/waybionic_hardware/waybionic_hardware/__init__.py new file mode 100644 index 0000000..af384f6 --- /dev/null +++ b/waybionic_hardware/waybionic_hardware/__init__.py @@ -0,0 +1 @@ +"""WayBionic hardware transport nodes.""" \ No newline at end of file diff --git a/waybionic_hardware/waybionic_hardware/arduino_bridge.py b/waybionic_hardware/waybionic_hardware/arduino_bridge.py new file mode 100644 index 0000000..3e1527d --- /dev/null +++ b/waybionic_hardware/waybionic_hardware/arduino_bridge.py @@ -0,0 +1,425 @@ +import math +import threading +import time + +import rclpy +from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue +from rclpy.node import Node +from sensor_msgs.msg import JointState +from std_msgs.msg import String + + +JOINT_NAMES = [ + 'old_arm_base_yaw_joint', + 'old_arm_shoulder_pitch_joint', + 'old_arm_elbow_pitch_joint', + 'old_arm_wrist_roll_joint', +] +UPRIGHT_PHYSICAL_DEGREES = [90.0, 35.0, 151.5, 27.5] +HOME_PHYSICAL_DEGREES = UPRIGHT_PHYSICAL_DEGREES +SERVO_DIRECTIONS = [1.0, -1.0, 1.0, 1.0] +MODEL_HOME_OFFSETS_DEGREES = [0.0, 90.0, 0.0, 0.0] +PHYSICAL_LIMITS = [(0.0, 270.0), (0.0, 112.5), (90.0, 270.0), (0.0, 270.0)] +SEQUENCE = [ + [60.0, 95.0, 125.0, 60.0], + [45.0, 75.0, 200.0, 100.0], + [15.0, 100.0, 130.0, 5.0], + HOME_PHYSICAL_DEGREES, +] +RESPONSE_TIMEOUT_SECONDS = 8.0 +HANDSHAKE_TIMEOUT_SECONDS = 3.0 +IDLE_RESPONSE_TIMEOUT_SECONDS = 5.0 +FAULT_STATUSES = { + 'arduino-error', + 'connection-failed', + 'serial-read-failed', + 'serial-write-failed', + 'malformed-response', + 'target-rejected', +} + + +def model_radians_from_physical(degrees): + return [ + math.radians((physical - zero) / direction + offset) + for physical, zero, direction, offset in zip( + degrees, HOME_PHYSICAL_DEGREES, SERVO_DIRECTIONS, + MODEL_HOME_OFFSETS_DEGREES) + ] + + +def smoothstep(progress): + return 6.0 * progress**5 - 15.0 * progress**4 + 10.0 * progress**3 + + +class ArduinoBridge(Node): + def __init__(self): + super().__init__('waybionic_arduino_bridge') + + self.declare_parameter('port', '') + self.declare_parameter('baud', 115200) + self.declare_parameter('segment_duration', 3.0) + self.declare_parameter('dry_run', False) + + self.port = str(self.get_parameter('port').value) + self.baud = int(self.get_parameter('baud').value) + self.segment_duration = float(self.get_parameter('segment_duration').value) + self.dry_run = bool(self.get_parameter('dry_run').value) + + self.joint_publisher = self.create_publisher(JointState, 'joint_states', 10) + self.diagnostics_publisher = self.create_publisher( + DiagnosticArray, '/diagnostics', 10) + self.status_publisher = self.create_publisher( + String, '/old_arm_motion_test/status', 10) + self.command_subscription = self.create_subscription( + String, '/old_arm_motion_test/command', self.handle_command, 10) + + self.serial_port = None + self.serial_lock = threading.Lock() + self.state_lock = threading.RLock() + self.serial_reader = None + self.serial_stop = threading.Event() + self.connected = False + self.ready = False + self.faulted = False + self.last_status = 'starting' + self.last_error = '' + self.last_response_monotonic = None + self.awaiting_probe = False + self.probe_sent_monotonic = None + self.current = list(HOME_PHYSICAL_DEGREES) + self.start = list(self.current) + self.target = list(self.current) + self.motion_started = 0.0 + self.motion_duration = 0.0 + self.motion_active = False + self.sequence_index = 0 + self.sequence_active = False + self.command_kind = 'idle' + self.stop_requested = False + self.home_completed = False + + self.state_timer = self.create_timer(0.02, self.publish_estimated_state) + self.diagnostics_timer = self.create_timer(1.0, self.publish_diagnostics) + self.connect_to_arduino() + self.publish_estimated_state() + + def connect_to_arduino(self): + if self.dry_run: + self.connected = True + self.ready = True + self.last_response_monotonic = time.monotonic() + self.last_status = 'dry-run' + self.get_logger().warn('Running in dry-run mode; no Arduino commands will be sent.') + return + + if not self.port: + self.last_status = 'port-not-configured' + self.last_error = 'Set the port parameter, for example /dev/ttyACM0.' + self.get_logger().error(self.last_error) + return + + try: + import serial + + self.serial_port = serial.Serial(self.port, self.baud, timeout=0.1) + self.connected = True + self.ready = False + self.last_response_monotonic = time.monotonic() + self.last_status = 'connected' + self.serial_reader = threading.Thread(target=self.read_serial, daemon=True) + self.serial_reader.start() + self.send_line('ID') + self.get_logger().info(f'Connected to Arduino on {self.port} at {self.baud} baud.') + except Exception as error: + self.last_error = str(error) + self.last_status = 'connection-failed' + self.get_logger().error(f'Could not open Arduino port {self.port}: {error}') + + def read_serial(self): + while not self.serial_stop.is_set() and self.serial_port is not None: + try: + line = self.serial_port.readline().decode('ascii', errors='replace').strip() + if line: + self.last_response_monotonic = time.monotonic() + self.process_serial_line(line) + except Exception as error: + self._latch_fault('serial-read-failed', str(error)) + return + + def _write_hold(self): + if self.dry_run or self.serial_port is None: + return True + try: + with self.serial_lock: + self.serial_port.write(b'HOLD\n') + return True + except Exception as error: + self.last_error = str(error) + return False + + def _latch_fault(self, status, error): + with self.state_lock: + self._write_hold() + self.motion_active = False + self.sequence_active = False + self.stop_requested = True + self.faulted = True + self.connected = False + self.ready = False + self.last_error = error + self.last_status = status + + def process_serial_line(self, line): + with self.state_lock: + self.get_logger().debug(f'Arduino: {line}') + self.awaiting_probe = False + self.probe_sent_monotonic = None + if line.startswith('READY,IK4,1'): + if self.faulted: + self.get_logger().warning('Ignoring READY while a fault is latched.') + return + self.connected = True + self.ready = True + self.faulted = False + self.last_error = '' + self.last_status = 'ready' + return + if line.startswith('ERROR,'): + self._latch_fault('arduino-error', line[6:]) + return + if line == 'OK,ARRIVED': + if not self.motion_active or self.stop_requested: + self.get_logger().debug('Ignoring stale ARRIVED after cancellation.') + return + self.current = list(self.target) + self.motion_active = False + self.last_status = 'arrived' + if self.command_kind == 'HOME': + self.home_completed = True + if self.sequence_active and self.command_kind == 'RUN': + self.sequence_index += 1 + if self.sequence_index < len(SEQUENCE): + self.send_move(SEQUENCE[self.sequence_index], 'RUN') + else: + self.sequence_active = False + return + if line in ('OK,MOVE', 'OK,JOG'): + self.last_status = 'accepted' + return + if line in ('OK,HOLD', 'OK,SWITCH HOLD'): + self.motion_active = False + self.sequence_active = False + self.stop_requested = True + self.command_kind = 'STOP' + self.last_status = 'held' + return + + self._latch_fault('malformed-response', f'Unexpected response: {line}') + + def send_line(self, line): + with self.state_lock: + if self.dry_run: + self.last_status = 'dry-run-accepted' + return True + if not self.connected or self.serial_port is None: + self.last_status = 'not-connected' + return False + try: + with self.serial_lock: + self.serial_port.write((line + '\n').encode('ascii')) + return True + except Exception as error: + self._latch_fault('serial-write-failed', str(error)) + return False + + def handle_command(self, message): + with self.state_lock: + command = message.data.strip().upper() + if command == 'RUN': + if not self.connected or not self.ready or self.faulted: + self.last_status = 'run-rejected-not-ready' + return + if not self.home_completed: + self.last_status = 'run-rejected-home-required' + return + self.sequence_index = 0 + self.sequence_active = True + self.stop_requested = False + self.send_move(SEQUENCE[0], 'RUN') + elif command == 'HOME': + if not self.connected or not self.ready or self.faulted: + self.last_status = 'home-rejected-not-ready' + return + self.sequence_active = False + self.stop_requested = False + self.send_move(HOME_PHYSICAL_DEGREES, 'HOME') + elif command == 'STOP': + self.sequence_active = False + self.motion_active = False + self.stop_requested = True + self.command_kind = 'STOP' + if self.send_line('HOLD'): + if self.dry_run: + self.process_serial_line('OK,HOLD') + else: + self.last_status = 'hold-requested' + else: + self.last_status = 'hold-request-failed' + else: + self.get_logger().warn(f'Ignoring unknown motion command: {command}') + + def send_move(self, target, command_kind): + if any( + value < lower or value > upper + for value, (lower, upper) in zip(target, PHYSICAL_LIMITS) + ): + self._latch_fault( + 'target-rejected', 'Target exceeds Arduino software limits.') + return + + with self.state_lock: + self.start = list(self.current) + self.target = list(target) + self.motion_started = time.monotonic() + self.motion_duration = max(0.3, min(15.0, self.segment_duration)) + self.command_kind = command_kind + command = 'MOVE,' + ','.join(f'{value:.2f}' for value in target) + command += f',{int(self.motion_duration * 1000)}' + if self.send_line(command): + self.motion_active = True + self.last_status = 'move-requested' + + def publish_estimated_state(self): + with self.state_lock: + now = time.monotonic() + if not self.dry_run and self.connected: + if not self.ready and self.last_response_monotonic is not None and now - self.last_response_monotonic > HANDSHAKE_TIMEOUT_SECONDS: + self._latch_fault('handshake-timeout', 'Arduino handshake timed out.') + elif self.motion_active and self.last_response_monotonic is not None and now - self.last_response_monotonic > self.motion_duration + RESPONSE_TIMEOUT_SECONDS: + self._latch_fault('serial-read-failed', 'Arduino response watchdog timed out.') + elif self.ready and not self.motion_active and self.last_response_monotonic is not None: + if self.awaiting_probe and self.probe_sent_monotonic is not None and now - self.probe_sent_monotonic > HANDSHAKE_TIMEOUT_SECONDS: + self._latch_fault('idle-timeout', 'Arduino stopped responding while idle.') + elif not self.awaiting_probe and now - self.last_response_monotonic > IDLE_RESPONSE_TIMEOUT_SECONDS: + self.awaiting_probe = True + self.probe_sent_monotonic = now + self.send_line('ID') + + if self.motion_active: + progress = min( + (now - self.motion_started) / self.motion_duration, + 1.0, + ) + blend = smoothstep(progress) + self.current = [ + start + (target - start) * blend + for start, target in zip(self.start, self.target) + ] + estimated_current = list(self.current) + + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = JOINT_NAMES + message.position = model_radians_from_physical(estimated_current) + self.joint_publisher.publish(message) + + if self.dry_run and self.motion_active and time.monotonic() >= self.motion_started + self.motion_duration: + self.process_serial_line('OK,ARRIVED') + + def publish_diagnostics(self): + bridge_status = DiagnosticStatus() + bridge_status.name = 'waybionic_arduino_bridge' + bridge_status.level = ( + DiagnosticStatus.ERROR + if self.faulted or not self.connected or self.last_status in FAULT_STATUSES + else DiagnosticStatus.WARN if not self.ready else DiagnosticStatus.OK + ) + bridge_status.message = self.last_status + bridge_status.values = [ + KeyValue(key='port', value=self.port or 'not configured'), + KeyValue(key='baud', value=str(self.baud)), + KeyValue(key='mode', value='dry-run' if self.dry_run else 'physical'), + KeyValue(key='error', value=self.last_error), + ] + + subscriber_count = self.count_subscribers('joint_states') + output_status = DiagnosticStatus() + output_status.name = 'waybionic_joint_state_output' + output_status.level = ( + DiagnosticStatus.OK if subscriber_count > 0 else DiagnosticStatus.ERROR) + output_status.message = ( + 'robot_state_publisher connected' + if subscriber_count > 0 else + 'No subscribers on joint_states; RViz cannot update') + output_status.values = [ + KeyValue(key='subscriber_count', value=str(subscriber_count)), + KeyValue(key='topic', value='joint_states'), + ] + + feedback_status = DiagnosticStatus() + feedback_status.name = 'waybionic_servo_feedback' + feedback_status.level = DiagnosticStatus.WARN + feedback_status.message = ( + 'No measured servo feedback; /joint_states is estimated') + feedback_status.values = [ + KeyValue(key='verification', value='command timeline only'), + KeyValue(key='servos', value='individual servo connection unknown'), + ] + + message = DiagnosticArray() + message.header.stamp = self.get_clock().now().to_msg() + message.status = [bridge_status, output_status, feedback_status] + self.diagnostics_publisher.publish(message) + + if self.faulted or bridge_status.level == DiagnosticStatus.ERROR: + motion_status = 'FAULT' + elif not self.ready: + motion_status = 'CONNECTING' + elif self.motion_active: + motion_status = 'RUNNING' + elif self.last_status == 'arrived': + motion_status = 'COMPLETE' + elif self.last_status == 'hold-requested': + motion_status = 'STOP_REQUESTED' + elif self.last_status == 'held': + motion_status = 'STOPPED' + elif self.ready and not self.home_completed: + motion_status = 'HOME_REQUIRED' + else: + motion_status = 'READY' + status_message = String() + status_message.data = motion_status + self.status_publisher.publish(status_message) + + def destroy_node(self): + with self.state_lock: + if self.motion_active and (self.connected or self.serial_port is not None): + self._write_hold() + self.motion_active = False + self.sequence_active = False + self.stop_requested = True + self.serial_stop.set() + if self.serial_reader is not None and self.serial_reader.is_alive(): + self.serial_reader.join(timeout=0.5) + if self.serial_port is not None: + try: + self.serial_port.close() + except Exception: + pass + super().destroy_node() + + +def main(args=None): + rclpy.init(args=args) + node = ArduinoBridge() + try: + rclpy.spin(node) + finally: + node.destroy_node() + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/waybionic_motion_test/package.xml b/waybionic_motion_test/package.xml new file mode 100644 index 0000000..fe3a5bf --- /dev/null +++ b/waybionic_motion_test/package.xml @@ -0,0 +1,21 @@ + + + waybionic_motion_test + 0.0.1 + RViz visual motion testing for the WayBionic robot. + + WayBionic + Apache-2.0 + + rclpy + sensor_msgs + std_msgs + + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/waybionic_motion_test/resource/waybionic_motion_test b/waybionic_motion_test/resource/waybionic_motion_test new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_motion_test/setup.cfg b/waybionic_motion_test/setup.cfg new file mode 100644 index 0000000..2f16cb3 --- /dev/null +++ b/waybionic_motion_test/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/waybionic_motion_test + +[install] +install_scripts=$base/lib/waybionic_motion_test diff --git a/waybionic_motion_test/setup.py b/waybionic_motion_test/setup.py new file mode 100644 index 0000000..807515f --- /dev/null +++ b/waybionic_motion_test/setup.py @@ -0,0 +1,22 @@ +from setuptools import find_packages, setup + +package_name = 'waybionic_motion_test' + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(), + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + description='RViz visual motion testing for the WayBionic robot.', + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'motion_test = waybionic_motion_test.motion_test:main', + ], + }, +) diff --git a/waybionic_motion_test/test/__init__.py b/waybionic_motion_test/test/__init__.py new file mode 100644 index 0000000..c9d91bf --- /dev/null +++ b/waybionic_motion_test/test/__init__.py @@ -0,0 +1 @@ +# Test module for waybionic_motion_test diff --git a/waybionic_motion_test/test/test_motion_test.py b/waybionic_motion_test/test/test_motion_test.py new file mode 100644 index 0000000..50a78f0 --- /dev/null +++ b/waybionic_motion_test/test/test_motion_test.py @@ -0,0 +1,68 @@ +"""Minimal tests for motion test module.""" +import unittest +from types import SimpleNamespace + + +class TestMotionTest(unittest.TestCase): + """Test cases for MotionTest.""" + + def test_import(self): + """Test that the module can be imported.""" + try: + from waybionic_motion_test import motion_test # noqa: F401 + self.assertTrue(True) + except ImportError: + self.fail("Failed to import motion_test") + + def test_joint_names_match_old_arm_model(self): + """Joint states target the imported old-arm URDF joints.""" + from waybionic_motion_test.motion_test import JOINT_NAMES + + self.assertEqual(JOINT_NAMES, [ + 'old_arm_base_yaw_joint', + 'old_arm_shoulder_pitch_joint', + 'old_arm_elbow_pitch_joint', + 'old_arm_wrist_roll_joint', + ]) + + def test_upright_home_pose_is_the_initial_stepper_pose(self): + from waybionic_motion_test.motion_test import ( + UPRIGHT_PHYSICAL_DEGREES, + model_radians_from_physical, + ) + + self.assertEqual(UPRIGHT_PHYSICAL_DEGREES, [90.0, 35.0, 151.5, 27.5]) + self.assertEqual( + model_radians_from_physical(UPRIGHT_PHYSICAL_DEGREES), + [0.0, 1.5707963267948966, 0.0, 0.0], + ) + + def test_home_does_not_advance_into_sequence(self): + from std_msgs.msg import String + from waybionic_motion_test.motion_test import MotionTestNode + + node = MotionTestNode.__new__(MotionTestNode) + node.home = [0.0, 0.0, 0.0, 0.0] + node.current = list(node.home) + node.start = list(node.home) + node.target = list(node.home) + node.sequence = [[1.0, 1.0, 1.0, 1.0], [2.0, 2.0, 2.0, 2.0]] + node.sequence_index = 0 + node.sequence_playback = False + node.segment_duration = 1.0 + node.segment_elapsed = 0.0 + node.segment_active = False + node.publish_joint_state = lambda: None + node.status_publisher = SimpleNamespace(publish=lambda message: None) + + node.handle_command(String(data='HOME')) + node.segment_elapsed = node.segment_duration + node.update_trajectory() + + self.assertFalse(node.segment_active) + self.assertEqual(node.current, node.home) + self.assertEqual(node.sequence_index, 0) + + +if __name__ == '__main__': + unittest.main() diff --git a/waybionic_motion_test/test/test_smoke.py b/waybionic_motion_test/test/test_smoke.py new file mode 100644 index 0000000..9993f44 --- /dev/null +++ b/waybionic_motion_test/test/test_smoke.py @@ -0,0 +1,2 @@ +def test_smoke_passes(): + assert True diff --git a/waybionic_motion_test/waybionic_motion_test/__init__.py b/waybionic_motion_test/waybionic_motion_test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/waybionic_motion_test/waybionic_motion_test/motion_test.py b/waybionic_motion_test/waybionic_motion_test/motion_test.py new file mode 100644 index 0000000..45162c8 --- /dev/null +++ b/waybionic_motion_test/waybionic_motion_test/motion_test.py @@ -0,0 +1,152 @@ +import math + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +from std_msgs.msg import String + + +JOINT_NAMES = [ + 'old_arm_base_yaw_joint', + 'old_arm_shoulder_pitch_joint', + 'old_arm_elbow_pitch_joint', + 'old_arm_wrist_roll_joint', +] +JOINT_COUNT = len(JOINT_NAMES) +UPRIGHT_PHYSICAL_DEGREES = [90.0, 35.0, 151.5, 27.5] +HOME_PHYSICAL_DEGREES = UPRIGHT_PHYSICAL_DEGREES +SERVO_DIRECTIONS = [1.0, -1.0, 1.0, 1.0] +MODEL_HOME_OFFSETS_DEGREES = [0.0, 90.0, 0.0, 0.0] + + +def model_radians_from_physical(degrees): + """Convert calibrated Arduino servo angles into URDF model radians.""" + return [ + math.radians((physical - zero) / direction + offset) + for physical, zero, direction, offset in zip( + degrees, HOME_PHYSICAL_DEGREES, SERVO_DIRECTIONS, + MODEL_HOME_OFFSETS_DEGREES) + ] + + +def smoothstep(progress): + return 6.0 * progress**5 - 15.0 * progress**4 + 10.0 * progress**3 + + +class MotionTestNode(Node): + def __init__(self): + super().__init__('motion_test') + + self.publisher = self.create_publisher( + JointState, + 'joint_states', + 10, + ) + self.status_publisher = self.create_publisher( + String, '/old_arm_motion_test/status', 10) + self.command_subscription = self.create_subscription( + String, + '/old_arm_motion_test/command', + self.handle_command, + 10, + ) + + self.declare_parameter('segment_duration', 3.0) + self.segment_duration = float(self.get_parameter('segment_duration').value) + if self.segment_duration <= 0.0: + raise ValueError('segment_duration must be positive') + self.home = model_radians_from_physical(HOME_PHYSICAL_DEGREES) + self.current = list(self.home) + self.start = list(self.home) + self.target = list(self.home) + self.segment_elapsed = 0.0 + self.segment_active = False + self.sequence = [ + model_radians_from_physical([60.0, 95.0, 125.0, 60.0]), + model_radians_from_physical([45.0, 75.0, 200.0, 100.0]), + model_radians_from_physical([15.0, 100.0, 130.0, 5.0]), + self.home, + ] + self.sequence_index = 0 + self.sequence_playback = False + self.timer = self.create_timer(0.02, self.update_trajectory) + self.publish_joint_state() + self.publish_status('READY') + + self.get_logger().info( + 'Old-arm 4-DOF motion test started; publishing simulated commanded positions.') + + def handle_command(self, message): + command = message.data.strip().upper() + if command == 'RUN': + self.sequence_index = 0 + self.sequence_playback = True + self.start_segment(self.sequence[self.sequence_index]) + self.publish_status('RUNNING') + elif command == 'HOME': + self.sequence_playback = False + self.start_segment(self.home) + self.publish_status('HOME') + elif command == 'STOP': + self.sequence_playback = False + self.segment_active = False + self.target = list(self.current) + self.publish_joint_state() + self.publish_status('STOPPED') + + def start_segment(self, target): + self.start = list(self.current) + self.target = list(target) + self.segment_elapsed = 0.0 + self.segment_active = True + + def update_trajectory(self): + if not self.segment_active: + return + + self.segment_elapsed += 0.02 + progress = min(self.segment_elapsed / self.segment_duration, 1.0) + blend = smoothstep(progress) + self.current = [ + start + (target - start) * blend + for start, target in zip(self.start, self.target) + ] + self.publish_joint_state() + + if progress >= 1.0: + self.current = list(self.target) + if self.sequence_playback and self.sequence_index < len(self.sequence) - 1: + self.sequence_index += 1 + self.start_segment(self.sequence[self.sequence_index]) + else: + self.segment_active = False + self.publish_status('COMPLETE') + + def publish_status(self, status): + message = String() + message.data = status + self.status_publisher.publish(message) + + def publish_joint_state(self): + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + message.name = JOINT_NAMES + message.position = self.current + self.publisher.publish(message) + + +def main(args=None): + rclpy.init(args=args) + + node = MotionTestNode() + + try: + rclpy.spin(node) + finally: + node.destroy_node() + + if rclpy.ok(): + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/waybionic_rviz_plugins/CMakeLists.txt b/waybionic_rviz_plugins/CMakeLists.txt index 413e3f0..f96872f 100644 --- a/waybionic_rviz_plugins/CMakeLists.txt +++ b/waybionic_rviz_plugins/CMakeLists.txt @@ -27,10 +27,12 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS add_library(${PROJECT_NAME} SHARED include/waybionic_rviz_plugins/diagnostics_source.hpp include/waybionic_rviz_plugins/diagnostics_panel.hpp + include/waybionic_rviz_plugins/motion_test_panel.hpp include/waybionic_rviz_plugins/ros_diagnostics_source.hpp src/diagnostics_panel.cpp src/mock_diagnostics_source.cpp src/ros_diagnostics_source.cpp + src/motion_test_panel.cpp ) target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) diff --git a/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz b/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz index 170a097..0d39bab 100644 --- a/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz +++ b/waybionic_rviz_plugins/config/engineer_monitoring_view.rviz @@ -1,11 +1,22 @@ Panels: - Class: rviz_common/Displays + Help Height: 70 Name: Displays + Property Tree Widget: + Expanded: ~ + Splitter Ratio: 0.5 + Tree Height: 70 - Class: rviz_common/Views + Expanded: + - /Current View1 Name: Views + Splitter Ratio: 0.5 - Class: waybionic_rviz_plugins/DiagnosticsPanel Diagnostics Topic: /diagnostics Name: WayBionic Diagnostics + Use Mock Diagnostics: false + - Class: waybionic_rviz_plugins/MotionTestPanel + Name: MotionTestPanel Visualization Manager: Class: "" Displays: @@ -15,7 +26,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.03 + Line Width: 0.029999999329447746 Value: Lines Name: Grid Normal Cell Count: 0 @@ -29,10 +40,12 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/TF Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: true - Marker Scale: 0.4 + Marker Scale: 0.4000000059604645 Name: TF Show Arrows: true Show Axes: true @@ -44,6 +57,7 @@ Visualization Manager: - Alpha: 1 Class: rviz_default_plugins/RobotModel Collision Enabled: false + Description File: "" Description Source: Topic Description Topic: Depth: 5 @@ -52,8 +66,16 @@ Visualization Manager: Reliability Policy: Reliable Value: /robot_description Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: "" + Mass Properties: + Inertia: false + Mass: false Name: Robot Model - Robot Description: robot_description TF Prefix: "" Update Interval: 0 Value: true @@ -79,36 +101,39 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 1.8 + Distance: 15.149079322814941 Enable Stereo Rendering: - Stereo Eye Separation: 0.06 + Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: X: 0 Y: 0 - Z: 0.2 + Z: 0.20000000298023224 Focal Shape Fixed Size: true - Focal Shape Size: 0.05 + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.55 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.550000011920929 Target Frame: Value: Orbit (rviz) - Yaw: 5.4 + Yaw: 5.400000095367432 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 995 + Height: 1104 Hide Left Dock: false Hide Right Dock: false + MotionTestPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001a4000003fafc0200000004fb000000100044006900730070006c006100790073000000003b000000c7000000c700fffffffb0000000a00560069006500770073000000003b000000c3000000a000fffffffb0000002a00570061007900420069006f006e0069006300200044006900610067006e006f00730074006900630073010000003b000003280000030e00fffffffb0000001e004d006f00740069006f006e005400650073007400500061006e0065006c0100000369000000cc000000cc00ffffff000005d6000003fa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true + collapsed: false WayBionic Diagnostics: collapsed: false Width: 1920 - X: 0 - Y: 0 + X: -29 + Y: -31 diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp index ab45aa1..31ca7da 100644 --- a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/diagnostics_panel.hpp @@ -14,6 +14,8 @@ #include #include +#include +#include #include #include @@ -22,72 +24,78 @@ #include "waybionic_rviz_plugins/mock_diagnostics_source.hpp" class QButtonGroup; +class QProgressBar; class QPushButton; namespace waybionic_rviz_plugins { -class DiagnosticsPanel : public rviz_common::Panel -{ - Q_OBJECT + class DiagnosticsPanel : public rviz_common::Panel + { + Q_OBJECT -public: - explicit DiagnosticsPanel(QWidget * parent = nullptr); - ~DiagnosticsPanel() override; + public: + explicit DiagnosticsPanel(QWidget *parent = nullptr); + ~DiagnosticsPanel() override; - void onInitialize() override; - void save(rviz_common::Config config) const override; - void load(const rviz_common::Config & config) override; + void onInitialize() override; + void save(rviz_common::Config config) const override; + void load(const rviz_common::Config &config) override; -private: - void buildUi(); - void configureSource(bool use_mock_diagnostics); - bool readUseMockDiagnosticsParameter(bool default_value); - std::string readDiagnosticsTopicParameter(const std::string & default_value); - void refresh(); - void setMockDiagnosticsState(MockDiagnosticsState mode); - void setUseMockDiagnostics(bool use_mock_diagnostics); - void updateSystemStatus( - const DiagnosticsSource & source, - const std::vector & messages, - const rclcpp::Time & now); - void updateTelemetryTable(const std::vector & messages, const rclcpp::Time & now); - void updateAlerts(const std::vector & messages); - void updateSourceControls(); - void clearAlerts(); + private: + void buildUi(); + void buildMovementTestUi(QVBoxLayout *root_layout); + void configureSource(bool use_mock_diagnostics); + bool readUseMockDiagnosticsParameter(bool default_value); + std::string readDiagnosticsTopicParameter(const std::string &default_value); + void refresh(); + void setMockDiagnosticsState(MockDiagnosticsState mode); + void setUseMockDiagnostics(bool use_mock_diagnostics); + void updateSystemStatus( + const DiagnosticsSource &source, + const std::vector &messages, + const rclcpp::Time &now); + void updateTelemetryTable(const std::vector &messages, const rclcpp::Time &now); + void updateAlerts(const std::vector &messages); + void updateSourceControls(); + void clearAlerts(); - QString statusColor(DiagnosticStatus status) const; - QString rowBackground(DiagnosticStatus status) const; - QString ageText(const rclcpp::Time & timestamp, const rclcpp::Time & now) const; - QString optionalText(const std::optional & value) const; - QString alertText(const DiagnosticMessage & message) const; + QString statusColor(DiagnosticStatus status) const; + QString rowBackground(DiagnosticStatus status) const; + QString ageText(const rclcpp::Time ×tamp, const rclcpp::Time &now) const; + QString optionalText(const std::optional &value) const; + QString alertText(const DiagnosticMessage &message) const; - // Shared ownership so a refresh tick keeps its source alive even if a mode - // switch replaces the panel's source part-way through the tick. - std::shared_ptr diagnostics_source_; - std::shared_ptr mock_diagnostics_source_; - rclcpp::Node::SharedPtr rviz_node_; - rclcpp::Clock clock_{RCL_SYSTEM_TIME}; - std::string diagnostics_topic_{"/diagnostics"}; - bool use_mock_diagnostics_{true}; + // Shared ownership so a refresh tick keeps its source alive even if a mode + // switch replaces the panel's source part-way through the tick. + std::shared_ptr diagnostics_source_; + std::shared_ptr mock_diagnostics_source_; + rclcpp::Node::SharedPtr rviz_node_; + rclcpp::Publisher::SharedPtr motion_command_publisher_; + rclcpp::Clock clock_{RCL_SYSTEM_TIME}; + std::string diagnostics_topic_{"/diagnostics"}; + bool use_mock_diagnostics_{true}; - QTimer * refresh_timer_{nullptr}; - QCheckBox * use_mock_diagnostics_checkbox_{nullptr}; - QLabel * state_label_{nullptr}; - QLabel * last_updated_label_{nullptr}; - QLabel * source_label_{nullptr}; - QLabel * ros_connection_label_{nullptr}; - QLabel * heartbeat_label_{nullptr}; - QLabel * ui_mode_label_{nullptr}; - QLabel * safety_label_{nullptr}; - QLabel * alert_icon_label_{nullptr}; - QTableWidget * telemetry_table_{nullptr}; - QVBoxLayout * alerts_layout_{nullptr}; - QPushButton * normal_button_{nullptr}; - QPushButton * fault_button_{nullptr}; - QButtonGroup * mock_state_button_group_{nullptr}; -}; + QTimer *refresh_timer_{nullptr}; + QCheckBox *use_mock_diagnostics_checkbox_{nullptr}; + QLabel *state_label_{nullptr}; + QLabel *last_updated_label_{nullptr}; + QLabel *source_label_{nullptr}; + QLabel *ros_connection_label_{nullptr}; + QLabel *heartbeat_label_{nullptr}; + QLabel *ui_mode_label_{nullptr}; + QLabel *safety_label_{nullptr}; + QLabel *alert_icon_label_{nullptr}; + QTableWidget *telemetry_table_{nullptr}; + QVBoxLayout *alerts_layout_{nullptr}; + QPushButton *normal_button_{nullptr}; + QPushButton *fault_button_{nullptr}; + QPushButton *movement_test_button_{nullptr}; + QLabel *movement_test_status_{nullptr}; + QProgressBar *movement_test_progress_{nullptr}; + QButtonGroup *mock_state_button_group_{nullptr}; + }; -} // namespace waybionic_rviz_plugins +} // namespace waybionic_rviz_plugins -#endif // WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_PANEL_HPP_ +#endif // WAYBIONIC_RVIZ_PLUGINS__DIAGNOSTICS_PANEL_HPP_ diff --git a/waybionic_rviz_plugins/include/waybionic_rviz_plugins/motion_test_panel.hpp b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/motion_test_panel.hpp new file mode 100644 index 0000000..43f60d7 --- /dev/null +++ b/waybionic_rviz_plugins/include/waybionic_rviz_plugins/motion_test_panel.hpp @@ -0,0 +1,60 @@ +#ifndef WAYBIONIC_RVIZ_PLUGINS__MOTION_TEST_PANEL_HPP_ +#define WAYBIONIC_RVIZ_PLUGINS__MOTION_TEST_PANEL_HPP_ + +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include + +#include + +namespace waybionic_rviz_plugins +{ + + class MotionTestPanel : public rviz_common::Panel + { + Q_OBJECT + + public: + explicit MotionTestPanel(QWidget *parent = nullptr); + ~MotionTestPanel() override; + + void onInitialize() override; + + private: + void buildUi(); + + void runTest(); + void moveHome(); + void stopTest(); + + void publishCommand(const std::string &command); + void handleStatus(const std_msgs::msg::String::SharedPtr message); + + QPushButton *run_button_{nullptr}; + QPushButton *home_button_{nullptr}; + QPushButton *stop_button_{nullptr}; + + QLabel *status_label_{nullptr}; + QLabel *position_label_{nullptr}; + QLabel *result_label_{nullptr}; + + rclcpp::Node::SharedPtr ros_node_; + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + std::thread executor_thread_; + rclcpp::Publisher::SharedPtr command_publisher_; + rclcpp::Subscription::SharedPtr status_subscription_; + bool test_running_{false}; + }; + +} // namespace waybionic_rviz_plugins + +#endif // WAYBIONIC_RVIZ_PLUGINS__MOTION_TEST_PANEL_HPP_ diff --git a/waybionic_rviz_plugins/package.xml b/waybionic_rviz_plugins/package.xml index 5b3285c..63bbb2d 100644 --- a/waybionic_rviz_plugins/package.xml +++ b/waybionic_rviz_plugins/package.xml @@ -12,6 +12,8 @@ pluginlib qtbase5-dev rclcpp + sensor_msgs + std_msgs rviz_common rviz_default_plugins diff --git a/waybionic_rviz_plugins/plugin_description.xml b/waybionic_rviz_plugins/plugin_description.xml index faa00a6..d6dc67d 100644 --- a/waybionic_rviz_plugins/plugin_description.xml +++ b/waybionic_rviz_plugins/plugin_description.xml @@ -7,4 +7,13 @@ WayBionic ground station diagnostics, telemetry, and alerts panel for RViz2. + + + + WayBionic visual robot motion testing panel. + + diff --git a/waybionic_rviz_plugins/src/diagnostics_panel.cpp b/waybionic_rviz_plugins/src/diagnostics_panel.cpp index ac0fde3..a716aa7 100644 --- a/waybionic_rviz_plugins/src/diagnostics_panel.cpp +++ b/waybionic_rviz_plugins/src/diagnostics_panel.cpp @@ -15,8 +15,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -30,10 +32,10 @@ namespace waybionic_rviz_plugins { -namespace -{ + namespace + { -constexpr const char * kPanelStyle = R"( + constexpr const char *kPanelStyle = R"( QWidget { background-color: #071019; color: #e8f1f8; @@ -95,429 +97,510 @@ QHeaderView::section { } )"; -QLabel * makeTitle(const QString & text) -{ - auto * label = new QLabel(text); - label->setObjectName("PanelTitle"); - return label; -} - -QLabel * makeMuted(const QString & text) -{ - auto * label = new QLabel(text); - label->setObjectName("Muted"); - return label; -} + QLabel *makeTitle(const QString &text) + { + auto *label = new QLabel(text); + label->setObjectName("PanelTitle"); + return label; + } -QFrame * makeCard() -{ - auto * card = new QFrame(); - card->setObjectName("Card"); - return card; -} + QLabel *makeMuted(const QString &text) + { + auto *label = new QLabel(text); + label->setObjectName("Muted"); + return label; + } -} // namespace + QFrame *makeCard() + { + auto *card = new QFrame(); + card->setObjectName("Card"); + return card; + } -DiagnosticsPanel::DiagnosticsPanel(QWidget * parent) -: rviz_common::Panel(parent) -{ - buildUi(); - configureSource(use_mock_diagnostics_); -} + } // namespace -DiagnosticsPanel::~DiagnosticsPanel() -{ - // Stop the timer before the members it reads are destroyed, then detach the - // live subscription so no executor callback outlives the panel. - if (refresh_timer_ != nullptr) { - refresh_timer_->stop(); - } - if (diagnostics_source_) { - diagnostics_source_->stop(); + DiagnosticsPanel::DiagnosticsPanel(QWidget *parent) + : rviz_common::Panel(parent) + { + buildUi(); + configureSource(use_mock_diagnostics_); } -} -void DiagnosticsPanel::onInitialize() -{ - if (auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock()) { - rviz_node_ = ros_node_abstraction->get_raw_node(); + DiagnosticsPanel::~DiagnosticsPanel() + { + // Stop the timer before the members it reads are destroyed, then detach the + // live subscription so no executor callback outlives the panel. + if (refresh_timer_ != nullptr) + { + refresh_timer_->stop(); + } + if (diagnostics_source_) + { + diagnostics_source_->stop(); + } } - refresh_timer_ = new QTimer(this); - connect(refresh_timer_, &QTimer::timeout, this, [this]() { refresh(); }); - refresh_timer_->start(1000); + void DiagnosticsPanel::onInitialize() + { + if (auto ros_node_abstraction = getDisplayContext()->getRosNodeAbstraction().lock()) + { + rviz_node_ = ros_node_abstraction->get_raw_node(); + motion_command_publisher_ = rviz_node_->create_publisher( + "/old_arm_motion_test/command", rclcpp::QoS(10)); + } + + refresh_timer_ = new QTimer(this); + connect(refresh_timer_, &QTimer::timeout, this, [this]() + { refresh(); }); + refresh_timer_->start(1000); - // Re-apply launch parameters after RViz finishes load() so use_mock_diagnostics:=false wins. - QTimer::singleShot(0, this, [this]() { + // Re-apply launch parameters after RViz finishes load() so use_mock_diagnostics:=false wins. + QTimer::singleShot(0, this, [this]() + { diagnostics_topic_ = readDiagnosticsTopicParameter(diagnostics_topic_); configureSource(readUseMockDiagnosticsParameter(use_mock_diagnostics_)); - refresh(); - }); -} + refresh(); }); + } -void DiagnosticsPanel::save(rviz_common::Config config) const -{ - rviz_common::Panel::save(config); - config.mapSetValue("Use Mock Diagnostics", use_mock_diagnostics_); - config.mapSetValue("Diagnostics Topic", QString::fromStdString(diagnostics_topic_)); -} + void DiagnosticsPanel::save(rviz_common::Config config) const + { + rviz_common::Panel::save(config); + config.mapSetValue("Use Mock Diagnostics", use_mock_diagnostics_); + config.mapSetValue("Diagnostics Topic", QString::fromStdString(diagnostics_topic_)); + } -void DiagnosticsPanel::load(const rviz_common::Config & config) -{ - rviz_common::Panel::load(config); + void DiagnosticsPanel::load(const rviz_common::Config &config) + { + rviz_common::Panel::load(config); - QString diagnostics_topic; - if (config.mapGetString("Diagnostics Topic", &diagnostics_topic)) { - diagnostics_topic_ = diagnostics_topic.toStdString(); + QString diagnostics_topic; + if (config.mapGetString("Diagnostics Topic", &diagnostics_topic)) + { + diagnostics_topic_ = diagnostics_topic.toStdString(); + } } -} -void DiagnosticsPanel::buildUi() -{ - setStyleSheet(kPanelStyle); - setMinimumWidth(420); - - auto * root_layout = new QVBoxLayout(this); - root_layout->setContentsMargins(10, 10, 10, 10); - root_layout->setSpacing(10); - - auto * header_card = makeCard(); - auto * header_layout = new QVBoxLayout(header_card); - header_layout->setSpacing(8); - - auto * title = makeTitle("WayBionic Engineering Monitor"); - state_label_ = new QLabel("Current State: NORMAL"); - state_label_->setObjectName("StateNormal"); - last_updated_label_ = makeMuted("Last updated: --"); - - use_mock_diagnostics_checkbox_ = new QCheckBox("Use Mock Diagnostics"); - use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); - use_mock_diagnostics_checkbox_->setToolTip( - "Checked: local mock validation states. Unchecked: subscribe to live ROS 2 diagnostics."); - connect(use_mock_diagnostics_checkbox_, &QCheckBox::toggled, this, [this](const bool checked) { - setUseMockDiagnostics(checked); - }); - - auto * button_row = new QHBoxLayout(); - normal_button_ = new QPushButton("Mock Normal"); - normal_button_->setCheckable(true); - normal_button_->setChecked(true); - fault_button_ = new QPushButton("Mock Fault"); - fault_button_->setCheckable(true); - - mock_state_button_group_ = new QButtonGroup(this); - mock_state_button_group_->setExclusive(true); - mock_state_button_group_->addButton(normal_button_); - mock_state_button_group_->addButton(fault_button_); - connect(normal_button_, &QPushButton::clicked, this, [this]() { - setMockDiagnosticsState(MockDiagnosticsState::Normal); - }); - connect(fault_button_, &QPushButton::clicked, this, [this]() { - setMockDiagnosticsState(MockDiagnosticsState::Fault); - }); - - button_row->addWidget(normal_button_); - button_row->addWidget(fault_button_); - - header_layout->addWidget(title); - header_layout->addWidget(use_mock_diagnostics_checkbox_); - header_layout->addLayout(button_row); - header_layout->addWidget(state_label_); - header_layout->addWidget(last_updated_label_); - root_layout->addWidget(header_card); - - auto * system_card = makeCard(); - auto * system_layout = new QGridLayout(system_card); - system_layout->setVerticalSpacing(6); - system_layout->addWidget(makeTitle("System Status"), 0, 0, 1, 2); - - source_label_ = new QLabel("Mock"); - ros_connection_label_ = new QLabel("Local mock diagnostics"); - heartbeat_label_ = new QLabel("OK"); - ui_mode_label_ = new QLabel("Monitoring only"); - safety_label_ = new QLabel("No motor commands sent from this RViz panel"); - safety_label_->setWordWrap(true); - - const std::vector> rows = { - {"Diagnostic Source", source_label_}, - {"ROS 2 Connection", ros_connection_label_}, - {"Backend Heartbeat", heartbeat_label_}, - {"UI Mode", ui_mode_label_}, - {"Safety Note", safety_label_}, - }; - - int row = 1; - for (const auto & [name, value] : rows) { - system_layout->addWidget(makeMuted(name), row, 0); - system_layout->addWidget(value, row, 1); - ++row; - } - root_layout->addWidget(system_card); - - auto * table_card = makeCard(); - auto * table_layout = new QVBoxLayout(table_card); - table_layout->addWidget(makeTitle("Telemetry + Live Values")); - - telemetry_table_ = new QTableWidget(0, 6); - telemetry_table_->setHorizontalHeaderLabels({"Signal", "Status", "Value", "Unit", "Last Updated", "Message"}); - telemetry_table_->setEditTriggers(QAbstractItemView::NoEditTriggers); - telemetry_table_->setSelectionBehavior(QAbstractItemView::SelectRows); - telemetry_table_->verticalHeader()->setVisible(false); - telemetry_table_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); - telemetry_table_->horizontalHeader()->setMinimumSectionSize(80); - table_layout->addWidget(telemetry_table_); - root_layout->addWidget(table_card, 1); - - auto * alerts_card = makeCard(); - auto * alerts_root = new QVBoxLayout(alerts_card); - auto * alerts_title_row = new QHBoxLayout(); - alerts_title_row->addWidget(makeTitle("Current Alerts"), 1); - alert_icon_label_ = new QLabel(""); - alert_icon_label_->setStyleSheet("color: #ff4d5e; font-size: 28px; font-weight: 900;"); - alerts_title_row->addWidget(alert_icon_label_); - alerts_root->addLayout(alerts_title_row); - - alerts_layout_ = new QVBoxLayout(); - alerts_layout_->setSpacing(6); - alerts_root->addLayout(alerts_layout_); - alerts_root->addStretch(1); - root_layout->addWidget(alerts_card); -} + void DiagnosticsPanel::buildUi() + { + setStyleSheet(kPanelStyle); + setMinimumWidth(420); -void DiagnosticsPanel::configureSource(const bool use_mock_diagnostics) -{ - use_mock_diagnostics_ = use_mock_diagnostics; - - // Detach the outgoing source before releasing it. Retiring the subscription - // first means an executor callback can no longer publish into a source the - // panel has stopped using, and it prevents a second subscription from - // existing alongside the old one. - auto retired_source = std::move(diagnostics_source_); - if (retired_source) { - retired_source->stop(); - } - mock_diagnostics_source_.reset(); + auto *root_layout = new QVBoxLayout(this); + root_layout->setContentsMargins(10, 10, 10, 10); + root_layout->setSpacing(10); - if (use_mock_diagnostics_ || !rviz_node_) { - mock_diagnostics_source_ = std::make_shared(); - diagnostics_source_ = mock_diagnostics_source_; - } else { - diagnostics_source_ = std::make_shared(rviz_node_, diagnostics_topic_); - } + auto *header_card = makeCard(); + auto *header_layout = new QVBoxLayout(header_card); + header_layout->setSpacing(8); - updateSourceControls(); + auto *title = makeTitle("WayBionic Engineering Monitor"); + state_label_ = new QLabel("Current State: NORMAL"); + state_label_->setObjectName("StateNormal"); + last_updated_label_ = makeMuted("Last updated: --"); - // Released only after the replacement is installed, so no refresh can observe - // a panel without a source. - retired_source.reset(); -} + use_mock_diagnostics_checkbox_ = new QCheckBox("Use Mock Diagnostics"); + use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); + use_mock_diagnostics_checkbox_->setToolTip( + "Checked: local mock validation states. Unchecked: subscribe to live ROS 2 diagnostics."); + connect(use_mock_diagnostics_checkbox_, &QCheckBox::toggled, this, [this](const bool checked) + { setUseMockDiagnostics(checked); }); + + auto *button_row = new QHBoxLayout(); + normal_button_ = new QPushButton("Mock Normal"); + normal_button_->setCheckable(true); + normal_button_->setChecked(true); + fault_button_ = new QPushButton("Mock Fault"); + fault_button_->setCheckable(true); + + mock_state_button_group_ = new QButtonGroup(this); + mock_state_button_group_->setExclusive(true); + mock_state_button_group_->addButton(normal_button_); + mock_state_button_group_->addButton(fault_button_); + connect(normal_button_, &QPushButton::clicked, this, [this]() + { setMockDiagnosticsState(MockDiagnosticsState::Normal); }); + connect(fault_button_, &QPushButton::clicked, this, [this]() + { setMockDiagnosticsState(MockDiagnosticsState::Fault); }); + + button_row->addWidget(normal_button_); + button_row->addWidget(fault_button_); + + header_layout->addWidget(title); + header_layout->addWidget(use_mock_diagnostics_checkbox_); + header_layout->addLayout(button_row); + header_layout->addWidget(state_label_); + header_layout->addWidget(last_updated_label_); + root_layout->addWidget(header_card); + + auto *system_card = makeCard(); + auto *system_layout = new QGridLayout(system_card); + system_layout->setVerticalSpacing(6); + system_layout->addWidget(makeTitle("System Status"), 0, 0, 1, 2); + + source_label_ = new QLabel("Mock"); + ros_connection_label_ = new QLabel("Local mock diagnostics"); + heartbeat_label_ = new QLabel("OK"); + ui_mode_label_ = new QLabel("Monitoring only"); + safety_label_ = new QLabel("No motor commands sent from this RViz panel"); + safety_label_->setWordWrap(true); + + const std::vector> rows = { + {"Diagnostic Source", source_label_}, + {"ROS 2 Connection", ros_connection_label_}, + {"Backend Heartbeat", heartbeat_label_}, + {"UI Mode", ui_mode_label_}, + {"Safety Note", safety_label_}, + }; -bool DiagnosticsPanel::readUseMockDiagnosticsParameter(const bool default_value) -{ - if (!rviz_node_) { - return default_value; + int row = 1; + for (const auto &[name, value] : rows) + { + system_layout->addWidget(makeMuted(name), row, 0); + system_layout->addWidget(value, row, 1); + ++row; + } + root_layout->addWidget(system_card); + + auto *table_card = makeCard(); + auto *table_layout = new QVBoxLayout(table_card); + table_layout->addWidget(makeTitle("Telemetry + Live Values")); + + telemetry_table_ = new QTableWidget(0, 6); + telemetry_table_->setHorizontalHeaderLabels({"Signal", "Status", "Value", "Unit", "Last Updated", "Message"}); + telemetry_table_->setEditTriggers(QAbstractItemView::NoEditTriggers); + telemetry_table_->setSelectionBehavior(QAbstractItemView::SelectRows); + telemetry_table_->verticalHeader()->setVisible(false); + telemetry_table_->horizontalHeader()->setSectionResizeMode(QHeaderView::Stretch); + telemetry_table_->horizontalHeader()->setMinimumSectionSize(80); + telemetry_table_->setMaximumHeight(220); + telemetry_table_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + table_layout->addWidget(telemetry_table_); + root_layout->addWidget(table_card); + + auto *alerts_card = makeCard(); + auto *alerts_root = new QVBoxLayout(alerts_card); + auto *alerts_title_row = new QHBoxLayout(); + alerts_title_row->addWidget(makeTitle("Current Alerts"), 1); + alert_icon_label_ = new QLabel(""); + alert_icon_label_->setStyleSheet("color: #ff4d5e; font-size: 28px; font-weight: 900;"); + alerts_title_row->addWidget(alert_icon_label_); + alerts_root->addLayout(alerts_title_row); + + alerts_layout_ = new QVBoxLayout(); + alerts_layout_->setSpacing(6); + alerts_root->addLayout(alerts_layout_); + alerts_card->setMaximumHeight(180); + alerts_card->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed); + root_layout->addWidget(alerts_card); + + buildMovementTestUi(root_layout); } - if (!rviz_node_->has_parameter("use_mock_diagnostics")) { - return rviz_node_->declare_parameter("use_mock_diagnostics", default_value); - } + void DiagnosticsPanel::buildMovementTestUi(QVBoxLayout *root_layout) + { + auto *movement_card = makeCard(); + auto *movement_layout = new QVBoxLayout(movement_card); + movement_layout->setSpacing(8); + + movement_layout->addWidget(makeTitle("Movement Test")); + + auto *description = makeMuted( + "Run a controlled movement sequence to visually inspect the robot before operation."); + description->setWordWrap(true); + movement_layout->addWidget(description); + + movement_test_button_ = new QPushButton("▶ Run Movement Test"); + movement_test_button_->setToolTip( + "Run the robot through its predefined movement test sequence."); + connect(movement_test_button_, &QPushButton::clicked, this, [this]() + { + if (!motion_command_publisher_) { + movement_test_status_->setText("ROS connection is not ready."); + return; + } - return rviz_node_->get_parameter("use_mock_diagnostics").as_bool(); -} + std_msgs::msg::String command; + command.data = "RUN"; + motion_command_publisher_->publish(command); + movement_test_status_->setText("Movement test command sent."); }); -std::string DiagnosticsPanel::readDiagnosticsTopicParameter(const std::string & default_value) -{ - if (!rviz_node_) { - return default_value; - } + movement_test_progress_ = new QProgressBar(); + movement_test_progress_->setRange(0, 100); + movement_test_progress_->setValue(0); + movement_test_progress_->setTextVisible(true); + + movement_test_status_ = makeMuted("Ready to run movement test."); - if (!rviz_node_->has_parameter("diagnostics_topic")) { - return rviz_node_->declare_parameter("diagnostics_topic", default_value); + movement_layout->addWidget(movement_test_button_); + movement_layout->addWidget(movement_test_progress_); + movement_layout->addWidget(movement_test_status_); + + root_layout->addWidget(movement_card); } - return rviz_node_->get_parameter("diagnostics_topic").as_string(); -} + void DiagnosticsPanel::configureSource(const bool use_mock_diagnostics) + { + use_mock_diagnostics_ = use_mock_diagnostics; + + // Detach the outgoing source before releasing it. Retiring the subscription + // first means an executor callback can no longer publish into a source the + // panel has stopped using, and it prevents a second subscription from + // existing alongside the old one. + auto retired_source = std::move(diagnostics_source_); + if (retired_source) + { + retired_source->stop(); + } + mock_diagnostics_source_.reset(); -void DiagnosticsPanel::refresh() -{ - // Pin the source for the whole tick so a mode switch cannot swap it out - // between reading the messages and rendering them. - const auto source = diagnostics_source_; - if (!source) { - return; + if (use_mock_diagnostics_ || !rviz_node_) + { + mock_diagnostics_source_ = std::make_shared(); + diagnostics_source_ = mock_diagnostics_source_; + } + else + { + diagnostics_source_ = std::make_shared(rviz_node_, diagnostics_topic_); + } + + updateSourceControls(); + + // Released only after the replacement is installed, so no refresh can observe + // a panel without a source. + retired_source.reset(); } - const auto now = clock_.now(); - const auto messages = source->messages(now); - updateSystemStatus(*source, messages, now); - updateTelemetryTable(messages, now); - updateAlerts(messages); -} + bool DiagnosticsPanel::readUseMockDiagnosticsParameter(const bool default_value) + { + if (!rviz_node_) + { + return default_value; + } -void DiagnosticsPanel::setMockDiagnosticsState(const MockDiagnosticsState mode) -{ - if (!mock_diagnostics_source_) { - return; + if (!rviz_node_->has_parameter("use_mock_diagnostics")) + { + return rviz_node_->declare_parameter("use_mock_diagnostics", default_value); + } + + return rviz_node_->get_parameter("use_mock_diagnostics").as_bool(); } - mock_diagnostics_source_->setMode(mode); - refresh(); -} + std::string DiagnosticsPanel::readDiagnosticsTopicParameter(const std::string &default_value) + { + if (!rviz_node_) + { + return default_value; + } -void DiagnosticsPanel::setUseMockDiagnostics(const bool use_mock_diagnostics) -{ - configureSource(use_mock_diagnostics); - refresh(); -} + if (!rviz_node_->has_parameter("diagnostics_topic")) + { + return rviz_node_->declare_parameter("diagnostics_topic", default_value); + } -void DiagnosticsPanel::updateSystemStatus( - const DiagnosticsSource & source, - const std::vector & messages, - const rclcpp::Time & now) -{ - const bool has_alert = - std::any_of(messages.begin(), messages.end(), [](const auto & message) { - return isAlertStatus(message.status); - }); - const bool has_stale = - std::any_of(messages.begin(), messages.end(), [](const auto & message) { - return message.status == DiagnosticStatus::Stale; - }); - - const bool waiting_for_live_diagnostics = - !use_mock_diagnostics_ && messages.size() == 1 && messages.front().signal_name == "diagnostics.topic"; - - if (waiting_for_live_diagnostics) { - state_label_->setText("Current State: WAITING FOR LIVE DIAGNOSTICS"); - } else { - state_label_->setText(has_alert ? "Current State: FAULT" : "Current State: NORMAL"); + return rviz_node_->get_parameter("diagnostics_topic").as_string(); } - state_label_->setObjectName(has_alert ? "StateFault" : "StateNormal"); - state_label_->style()->unpolish(state_label_); - state_label_->style()->polish(state_label_); - - source_label_->setText(QString::fromStdString(source.sourceName())); - ros_connection_label_->setText(QString::fromStdString(source.connectionStatus(now))); - heartbeat_label_->setText(has_stale ? "STALE" : "OK"); - heartbeat_label_->setStyleSheet(QString("color: %1; font-weight: 800;").arg(has_stale ? "#9aa4ad" : "#3ddc84")); - safety_label_->setStyleSheet(QString("color: %1; font-weight: 700;").arg(has_alert ? "#ff4d5e" : "#8ea3b1")); - - if (messages.empty()) { - last_updated_label_->setText("Last updated: --"); - return; + + void DiagnosticsPanel::refresh() + { + // Pin the source for the whole tick so a mode switch cannot swap it out + // between reading the messages and rendering them. + const auto source = diagnostics_source_; + if (!source) + { + return; + } + + const auto now = clock_.now(); + const auto messages = source->messages(now); + updateSystemStatus(*source, messages, now); + updateTelemetryTable(messages, now); + updateAlerts(messages); } - double latest_age = std::numeric_limits::max(); - for (const auto & message : messages) { - latest_age = std::min(latest_age, (now - message.timestamp).seconds()); + void DiagnosticsPanel::setMockDiagnosticsState(const MockDiagnosticsState mode) + { + if (!mock_diagnostics_source_) + { + return; + } + + mock_diagnostics_source_->setMode(mode); + refresh(); } - last_updated_label_->setText(QString("Last updated: %1s ago").arg(latest_age, 0, 'f', 1)); -} -void DiagnosticsPanel::updateSourceControls() -{ - if (use_mock_diagnostics_checkbox_ != nullptr) { - const QSignalBlocker blocker(use_mock_diagnostics_checkbox_); - use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); + void DiagnosticsPanel::setUseMockDiagnostics(const bool use_mock_diagnostics) + { + configureSource(use_mock_diagnostics); + refresh(); } - const bool mock_enabled = static_cast(mock_diagnostics_source_); - if (normal_button_ != nullptr) { - normal_button_->setEnabled(mock_enabled); - if (!mock_enabled) { - normal_button_->setChecked(false); + void DiagnosticsPanel::updateSystemStatus( + const DiagnosticsSource &source, + const std::vector &messages, + const rclcpp::Time &now) + { + const bool has_alert = + std::any_of(messages.begin(), messages.end(), [](const auto &message) + { return isAlertStatus(message.status); }); + const bool has_stale = + std::any_of(messages.begin(), messages.end(), [](const auto &message) + { return message.status == DiagnosticStatus::Stale; }); + + const bool waiting_for_live_diagnostics = + !use_mock_diagnostics_ && messages.size() == 1 && messages.front().signal_name == "diagnostics.topic"; + + if (waiting_for_live_diagnostics) + { + state_label_->setText("Current State: WAITING FOR LIVE DIAGNOSTICS"); } - } - if (fault_button_ != nullptr) { - fault_button_->setEnabled(mock_enabled); - if (!mock_enabled) { - fault_button_->setChecked(false); + else + { + state_label_->setText(has_alert ? "Current State: FAULT" : "Current State: NORMAL"); + } + state_label_->setObjectName(has_alert ? "StateFault" : "StateNormal"); + state_label_->style()->unpolish(state_label_); + state_label_->style()->polish(state_label_); + + source_label_->setText(QString::fromStdString(source.sourceName())); + ros_connection_label_->setText(QString::fromStdString(source.connectionStatus(now))); + heartbeat_label_->setText(has_stale ? "STALE" : "OK"); + heartbeat_label_->setStyleSheet(QString("color: %1; font-weight: 800;").arg(has_stale ? "#9aa4ad" : "#3ddc84")); + safety_label_->setStyleSheet(QString("color: %1; font-weight: 700;").arg(has_alert ? "#ff4d5e" : "#8ea3b1")); + + if (messages.empty()) + { + last_updated_label_->setText("Last updated: --"); + return; + } + + double latest_age = std::numeric_limits::max(); + for (const auto &message : messages) + { + latest_age = std::min(latest_age, (now - message.timestamp).seconds()); } + last_updated_label_->setText(QString("Last updated: %1s ago").arg(latest_age, 0, 'f', 1)); } -} -void DiagnosticsPanel::updateTelemetryTable( - const std::vector & messages, - const rclcpp::Time & now) -{ - telemetry_table_->setRowCount(static_cast(messages.size())); - - for (int row = 0; row < static_cast(messages.size()); ++row) { - const auto & message = messages.at(row); - const QStringList values = { - QString::fromStdString(message.signal_name), - toString(message.status), - optionalText(message.value), - optionalText(message.unit), - ageText(message.timestamp, now), - optionalText(message.alert_message), - }; + void DiagnosticsPanel::updateSourceControls() + { + if (use_mock_diagnostics_checkbox_ != nullptr) + { + const QSignalBlocker blocker(use_mock_diagnostics_checkbox_); + use_mock_diagnostics_checkbox_->setChecked(use_mock_diagnostics_); + } - for (int column = 0; column < values.size(); ++column) { - auto * item = new QTableWidgetItem(values.at(column)); - item->setBackground(QColor(rowBackground(message.status))); - item->setForeground(QColor(column == 1 ? statusColor(message.status) : "#e8f1f8")); - if (column == 1 || column == 2 || column == 3 || column == 4) { - item->setTextAlignment(Qt::AlignCenter); + const bool mock_enabled = static_cast(mock_diagnostics_source_); + if (normal_button_ != nullptr) + { + normal_button_->setEnabled(mock_enabled); + if (!mock_enabled) + { + normal_button_->setChecked(false); } - if (message.status != DiagnosticStatus::Ok && (column == 0 || column == 1 || column == 5)) { - auto font = item->font(); - font.setBold(true); - item->setFont(font); + } + if (fault_button_ != nullptr) + { + fault_button_->setEnabled(mock_enabled); + if (!mock_enabled) + { + fault_button_->setChecked(false); } - telemetry_table_->setItem(row, column, item); } } -} -void DiagnosticsPanel::updateAlerts(const std::vector & messages) -{ - clearAlerts(); - - bool has_alert = false; - for (const auto & message : messages) { - if (!isAlertStatus(message.status)) { - continue; - } - - has_alert = true; - auto * label = new QLabel(alertText(message)); - label->setWordWrap(true); - label->setStyleSheet(QString( - "background-color: rgba(255, 77, 94, 0.18);" - "border: 1px solid %1;" - "border-radius: 6px;" - "color: #e8f1f8;" - "font-weight: 800;" - "padding: 8px;").arg(statusColor(message.status))); - alerts_layout_->addWidget(label); + void DiagnosticsPanel::updateTelemetryTable( + const std::vector &messages, + const rclcpp::Time &now) + { + telemetry_table_->setRowCount(static_cast(messages.size())); + + for (int row = 0; row < static_cast(messages.size()); ++row) + { + const auto &message = messages.at(row); + const QStringList values = { + QString::fromStdString(message.signal_name), + toString(message.status), + optionalText(message.value), + optionalText(message.unit), + ageText(message.timestamp, now), + optionalText(message.alert_message), + }; + + for (int column = 0; column < values.size(); ++column) + { + auto *item = new QTableWidgetItem(values.at(column)); + item->setBackground(QColor(rowBackground(message.status))); + item->setForeground(QColor(column == 1 ? statusColor(message.status) : "#e8f1f8")); + if (column == 1 || column == 2 || column == 3 || column == 4) + { + item->setTextAlignment(Qt::AlignCenter); + } + if (message.status != DiagnosticStatus::Ok && (column == 0 || column == 1 || column == 5)) + { + auto font = item->font(); + font.setBold(true); + item->setFont(font); + } + telemetry_table_->setItem(row, column, item); + } + } } - if (!has_alert) { - alert_icon_label_->setText(""); - auto * label = new QLabel("No active alerts"); - label->setStyleSheet("color: #3ddc84; font-size: 15px; font-weight: 800;"); - alerts_layout_->addWidget(label); - return; - } + void DiagnosticsPanel::updateAlerts(const std::vector &messages) + { + clearAlerts(); - alert_icon_label_->setText("!"); -} + bool has_alert = false; + for (const auto &message : messages) + { + if (!isAlertStatus(message.status)) + { + continue; + } -void DiagnosticsPanel::clearAlerts() -{ - while (alerts_layout_->count() > 0) { - auto * item = alerts_layout_->takeAt(0); - if (auto * widget = item->widget()) { - widget->deleteLater(); + has_alert = true; + auto *label = new QLabel(alertText(message)); + label->setWordWrap(true); + label->setStyleSheet(QString( + "background-color: rgba(255, 77, 94, 0.18);" + "border: 1px solid %1;" + "border-radius: 6px;" + "color: #e8f1f8;" + "font-weight: 800;" + "padding: 8px;") + .arg(statusColor(message.status))); + alerts_layout_->addWidget(label); + } + + if (!has_alert) + { + alert_icon_label_->setText(""); + auto *label = new QLabel("No active alerts"); + label->setStyleSheet("color: #3ddc84; font-size: 15px; font-weight: 800;"); + alerts_layout_->addWidget(label); + return; } - delete item; + + alert_icon_label_->setText("!"); } -} -QString DiagnosticsPanel::statusColor(const DiagnosticStatus status) const -{ - switch (status) { + void DiagnosticsPanel::clearAlerts() + { + while (alerts_layout_->count() > 0) + { + auto *item = alerts_layout_->takeAt(0); + if (auto *widget = item->widget()) + { + widget->deleteLater(); + } + delete item; + } + } + + QString DiagnosticsPanel::statusColor(const DiagnosticStatus status) const + { + switch (status) + { case DiagnosticStatus::Ok: return "#3ddc84"; case DiagnosticStatus::Warn: @@ -526,13 +609,14 @@ QString DiagnosticsPanel::statusColor(const DiagnosticStatus status) const return "#ff4d5e"; case DiagnosticStatus::Stale: return "#9aa4ad"; + } + return "#e8f1f8"; } - return "#e8f1f8"; -} -QString DiagnosticsPanel::rowBackground(const DiagnosticStatus status) const -{ - switch (status) { + QString DiagnosticsPanel::rowBackground(const DiagnosticStatus status) const + { + switch (status) + { case DiagnosticStatus::Fault: return "#241018"; case DiagnosticStatus::Warn: @@ -541,41 +625,44 @@ QString DiagnosticsPanel::rowBackground(const DiagnosticStatus status) const return "#151922"; case DiagnosticStatus::Ok: return "#09131d"; + } + return "#09131d"; } - return "#09131d"; -} - -QString DiagnosticsPanel::ageText(const rclcpp::Time & timestamp, const rclcpp::Time & now) const -{ - const double age_seconds = std::max(0.0, (now - timestamp).seconds()); - return QString("%1s ago").arg(age_seconds, 0, 'f', 1); -} -QString DiagnosticsPanel::optionalText(const std::optional & value) const -{ - if (!value.has_value() || value->empty()) { - return "-"; + QString DiagnosticsPanel::ageText(const rclcpp::Time ×tamp, const rclcpp::Time &now) const + { + const double age_seconds = std::max(0.0, (now - timestamp).seconds()); + return QString("%1s ago").arg(age_seconds, 0, 'f', 1); } - return QString::fromStdString(*value); -} -QString DiagnosticsPanel::alertText(const DiagnosticMessage & message) const -{ - if (message.signal_name == "board.temperature") { - return QString("FAULT - Board temperature high: %1 %2") - .arg(optionalText(message.value), optionalText(message.unit)); + QString DiagnosticsPanel::optionalText(const std::optional &value) const + { + if (!value.has_value() || value->empty()) + { + return "-"; + } + return QString::fromStdString(*value); } - if (message.signal_name == "imu.heartbeat") { - return "STALE - IMU heartbeat timeout"; - } + QString DiagnosticsPanel::alertText(const DiagnosticMessage &message) const + { + if (message.signal_name == "board.temperature") + { + return QString("FAULT - Board temperature high: %1 %2") + .arg(optionalText(message.value), optionalText(message.unit)); + } - return QString("%1 - %2: %3") - .arg(toString(message.status)) - .arg(QString::fromStdString(message.signal_name)) - .arg(optionalText(message.alert_message)); -} + if (message.signal_name == "imu.heartbeat") + { + return "STALE - IMU heartbeat timeout"; + } + + return QString("%1 - %2: %3") + .arg(toString(message.status)) + .arg(QString::fromStdString(message.signal_name)) + .arg(optionalText(message.alert_message)); + } -} // namespace waybionic_rviz_plugins +} // namespace waybionic_rviz_plugins PLUGINLIB_EXPORT_CLASS(waybionic_rviz_plugins::DiagnosticsPanel, rviz_common::Panel) diff --git a/waybionic_rviz_plugins/src/motion_test_panel.cpp b/waybionic_rviz_plugins/src/motion_test_panel.cpp new file mode 100644 index 0000000..7ccfacd --- /dev/null +++ b/waybionic_rviz_plugins/src/motion_test_panel.cpp @@ -0,0 +1,201 @@ +#include "waybionic_rviz_plugins/motion_test_panel.hpp" + +#include +#include +#include + +#include + +namespace waybionic_rviz_plugins +{ + + MotionTestPanel::MotionTestPanel(QWidget *parent) + : rviz_common::Panel(parent) + { + buildUi(); + } + + MotionTestPanel::~MotionTestPanel() + { + if (executor_) + { + executor_->cancel(); + } + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + + void MotionTestPanel::onInitialize() + { + ros_node_ = std::make_shared("waybionic_motion_test_panel"); + + command_publisher_ = ros_node_->create_publisher( + "/old_arm_motion_test/command", rclcpp::QoS(10)); + status_subscription_ = ros_node_->create_subscription( + "/old_arm_motion_test/status", rclcpp::QoS(10), + [this](const std_msgs::msg::String::SharedPtr message) + { handleStatus(message); }); + executor_ = std::make_shared(); + executor_->add_node(ros_node_); + executor_thread_ = std::thread([this]() + { executor_->spin(); }); + } + void MotionTestPanel::buildUi() + { + setMinimumWidth(360); + + auto *layout = new QVBoxLayout(this); + + auto *title = new QLabel("Old Arm Prototype Motion Test"); + title->setStyleSheet( + "font-size: 16px;" + "font-weight: 700;" + "padding: 6px;"); + + status_label_ = new QLabel("Status: READY"); + status_label_->setStyleSheet( + "color: #3ddc84;" + "font-weight: 800;" + "padding: 6px;"); + + position_label_ = new QLabel("Four-joint trajectory via /joint_states"); + + result_label_ = new QLabel("No test run yet."); + result_label_->setWordWrap(true); + + run_button_ = new QPushButton("▶ RUN MOTION TEST"); + home_button_ = new QPushButton("↩ HOME"); + stop_button_ = new QPushButton("■ STOP"); + + stop_button_->setEnabled(false); + + connect(run_button_, &QPushButton::clicked, this, [this]() + { runTest(); }); + + connect(home_button_, &QPushButton::clicked, this, [this]() + { moveHome(); }); + + connect(stop_button_, &QPushButton::clicked, this, [this]() + { stopTest(); }); + + auto *button_row = new QHBoxLayout(); + button_row->addWidget(home_button_); + button_row->addWidget(stop_button_); + + layout->addWidget(title); + layout->addWidget(status_label_); + layout->addWidget(position_label_); + layout->addWidget(run_button_); + layout->addLayout(button_row); + layout->addWidget(result_label_); + layout->addStretch(); + } + + void MotionTestPanel::runTest() + { + if (test_running_) + { + return; + } + + test_running_ = true; + + result_label_->setText("Running motion sequence..."); + status_label_->setText("RUNNING: synchronized 4-DOF sequence"); + + run_button_->setEnabled(false); + home_button_->setEnabled(false); + stop_button_->setEnabled(true); + + publishCommand("RUN"); + } + + void MotionTestPanel::moveHome() + { + test_running_ = false; + + status_label_->setText("Moving to HOME"); + result_label_->setText("Returning to home position..."); + + run_button_->setEnabled(false); + home_button_->setEnabled(false); + stop_button_->setEnabled(true); + + publishCommand("HOME"); + } + + void MotionTestPanel::stopTest() + { + test_running_ = false; + publishCommand("STOP"); + + status_label_->setText("STOP REQUESTED"); + result_label_->setText("Waiting for the Arduino to confirm HOLD."); + + run_button_->setEnabled(false); + home_button_->setEnabled(false); + stop_button_->setEnabled(false); + } + + void MotionTestPanel::publishCommand(const std::string &command) + { + if (!command_publisher_) + { + return; + } + + std_msgs::msg::String message; + message.data = command; + command_publisher_->publish(message); + } + + void MotionTestPanel::handleStatus(const std_msgs::msg::String::SharedPtr message) + { + const QString status = QString::fromStdString(message->data); + QMetaObject::invokeMethod(this, [this, status]() + { + if (status == "COMPLETE" || status == "READY") { + test_running_ = false; + status_label_->setText("READY"); + result_label_->setText(status == "COMPLETE" ? "Motion sequence complete." : "Ready."); + run_button_->setEnabled(true); + home_button_->setEnabled(true); + stop_button_->setEnabled(false); + } else if (status == "HOME_REQUIRED") { + test_running_ = false; + status_label_->setText("HOME REQUIRED"); + result_label_->setText("Move to HOME before running the sequence."); + run_button_->setEnabled(false); + home_button_->setEnabled(true); + stop_button_->setEnabled(false); + } else if (status == "CONNECTING") { + test_running_ = false; + status_label_->setText("CONNECTING"); + result_label_->setText("Waiting for the Arduino handshake."); + run_button_->setEnabled(false); + home_button_->setEnabled(false); + stop_button_->setEnabled(false); + } else if (status == "STOP_REQUESTED") { + test_running_ = false; + status_label_->setText("STOP REQUESTED"); + result_label_->setText("Waiting for the Arduino to confirm HOLD."); + run_button_->setEnabled(false); + home_button_->setEnabled(false); + stop_button_->setEnabled(false); + } else if (status == "STOPPED" || status == "FAULT" || status == "ERROR") { + test_running_ = false; + status_label_->setText(status); + result_label_->setText("Motion unavailable or stopped."); + run_button_->setEnabled(status == "STOPPED"); + home_button_->setEnabled(status == "STOPPED"); + stop_button_->setEnabled(false); + } }, Qt::QueuedConnection); + } + +} // namespace waybionic_rviz_plugins + +PLUGINLIB_EXPORT_CLASS( + waybionic_rviz_plugins::MotionTestPanel, + rviz_common::Panel)