How to do automated test when robot state doesn't change by a start signal (ros published topic)? - testing

Question

I have a ROS2 package that simulate a robot(machine) movements(actions) with Gazebo. Im able to launch the Gazebo and Rviz and correctly show he robot. So when I would like to simulate some robot (machine ) movements that are initialized with start and digging(machine movement) signals(that are actually ros2 topics) the robot(machine) is not performing (not digging).
So first this is the launch file
#!/usr/bin/env python3
import os
from os import path as osp
import shlex
import time
from backhoe_launch.launch_utils import include_launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions.execute_process import ExecuteProcess
from launch.substitutions import LaunchConfiguration
from ament_index_python import get_package_prefix
from launch import logging
def generate_launch_description():
logging.reset()
ros_pkg_path = get_package_prefix('backhoe_ros_pkg').replace(
"/install/", "/src/"
)
default_exp_dir = osp.join(ros_pkg_path, "experiments")
log_manage_txt = 'log_dirs.txt'
log_manage_path = osp.join(default_exp_dir, log_manage_txt)
start_time = time.strftime('%Y%m%d%H%M%S')
exp_result_dir = osp.join(default_exp_dir, start_time)
rosbag_dir = osp.join(exp_result_dir, 'rosbag')
rosbag_dir_separate = osp.join(exp_result_dir, 'rosbag_separate')
if not os.path.isdir(exp_result_dir):
os.makedirs(exp_result_dir)
logging.launch_config.log_dir = exp_result_dir
arguments = [
DeclareLaunchArgument(
'namespace',
default_value=['backhoe'],
),
DeclareLaunchArgument(
'points',
default_value=['/backhoe/livox/lidar'],
),
DeclareLaunchArgument(
'inverted_imu',
default_value=['True'],
),
DeclareLaunchArgument(
'experiment_name',
default_value=[''],
),
DeclareLaunchArgument(
'speed_type',
default_value=['low'],
),
DeclareLaunchArgument(
'record_topic',
default_value=['some'],
),
DeclareLaunchArgument(
'rosbag_dir',
default_value=[rosbag_dir],
),
DeclareLaunchArgument(
'rosbag_dir_separate',
default_value=[rosbag_dir_separate],
),
DeclareLaunchArgument(
'is_system_test',
default_value=['False'],
),
]
state_publisher_node = include_launch(
'backhoe_launch',
'backhoe_state_publisher_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
('inverted_imu', LaunchConfiguration('inverted_imu')),
],
)
perception_bring_up_node = include_launch(
'backhoe_launch',
'perception_bringup_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
('points', LaunchConfiguration('points')),
],
)
digging_perception_node = include_launch(
'backhoe_launch',
'digging_perception_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
],
)
dumper_perception_service_node = include_launch(
'backhoe_launch',
'dumper_perception_service_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
('input_points1', '/filtered/points'),
('input_points2', '/dup/dumper_perception/input/points'),
],
)
dumper_termination_service_node = include_launch(
'backhoe_launch',
'dumper_termination_service_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
('input_points1', '/filtered/points'),
('input_points2', '/dup/dumper_termination/input/points'),
],
)
state_machine_node = include_launch(
'backhoe_state_machine',
'backhoe_state_machine_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
('experiment_name', LaunchConfiguration('experiment_name')),
('speed_type', LaunchConfiguration('speed_type')),
],
)
assert state_machine_node
rosbag_record = include_launch(
'backhoe_launch',
'rosbag_record_launch.py',
launch_arguments=[
('record_topic', LaunchConfiguration('record_topic')),
('directory', LaunchConfiguration('rosbag_dir')),
('directory_separate', LaunchConfiguration('rosbag_dir_separate')),
],
)
system_test_node = include_launch(
'backhoe_system_test',
'backhoe_system_test_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
('is_system_test', LaunchConfiguration('is_system_test')),
],
)
userinput_node = include_launch(
'backhoe_interface',
'userinput_launch.py',
launch_arguments=[
('namespace', LaunchConfiguration('namespace')),
],
)
nodes = [
system_test_node,
state_publisher_node,
perception_bring_up_node,
digging_perception_node,
dumper_perception_service_node,
dumper_termination_service_node,
state_machine_node,
rosbag_record,
userinput_node,
]
log_name = [
ExecuteProcess(
cmd=shlex.split(f'echo {start_time} >> {log_manage_path}'),
shell=True,
output='screen',
on_exit=nodes,
)
]
return LaunchDescription([*arguments, *log_name])
So then the first start signal is working and robot machine move for 20 degree. This is the first start signal:
ros2 topic pub /robot/start_signal std_msgs/msg/Bool '{data: True}' -1
And I can see on the terminal the state change:
[logger]: Entering DumperDetection [rexas_dashboard_node.py-31] [INFO]
[1667976821.134913838] [backhoe.dashboard]: Looking up
diagnostics_toplevel_state from dashboard
And the Gazebo is listening on /robot/action to Action type massages and execute them . So we can confirm that with
ros2 topic echo /robot/action
boom_volt: data: 1.4500000476837158 arm_volt: data:
1.4500000476837158 bucket_volt: data: 1.4500000476837158 revolve_volt: data: 1.4500000476837158 caterpillar_left_volt:
data: 1.4500000476837158 caterpillar_right_volt: data:
1.4500000476837158
So far so good. But when execute the second command( this command should start robot-machine digging) the robot(machine) is not doing anything and also the state doesnt change. So this is the command
ros2 topic pub /robot/start_digging_signal std_msgs/msg/Bool '{data: True}' -1
and can see the
ros2 topic echo /robot/action
is empty (nothing to publish) and also the state is not changing. So my question is how can a find or develop automated test and perform some debugging to find the error. I mean what kind of test I can use or develop for such cases when the state (state machine) is not working and some ros topics (signals) not working?
Thanks