Using ROS2 Humble. I have been stuck on a custom controller issue for a while now. I'm pretty sure some ROS2 updates broke my package, as running things using mock_components/GenericSystem doesn't seem to work anymore. Interestingly, rqt_joint_trajectory_controller doesn't work on my controller anymore either.
The error
When launching with ros2 launch linear_slider_bringup linear_slider.launch.py use_mock_hardware:=true, everything successfully launches. I then plan and execute a new pose using RViz, receiving the following error:
[move_group-4] [INFO] [1723589509.274056329] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'linear_slider__manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-4] [INFO] [1723589509.300718025] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1723589509.300744548] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1723589509.300795754] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.01
[move_group-4] [INFO] [1723589509.302286655] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-4] [INFO] [1723589509.302339110] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1723589509.302355776] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-4] [INFO] [1723589509.302475309] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to linear_slider_controller
[ros2_control_node-2] [INFO] [1723589509.302950701] [linear_slider_controller]: Received new action goal
[ros2_control_node-2] [INFO] [1723589509.303008767] [linear_slider_controller]: Accepted new action goal
[move_group-4] [INFO] [1723589509.303150866] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: linear_slider_controller started execution
[move_group-4] [INFO] [1723589509.303176253] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
`[move_group-4] [WARN] [1723589518.271080092] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[move_group-4] [ERROR] [1723589518.271169625] [moveit_ros.trajectory_execution_manager]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 8.967761 seconds). Stopping trajectory.
[move_group-4] [INFO] [1723589518.271218034] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Cancelling execution for linear_slider_controller
[ros2_control_node-2] [INFO] [1723589518.271686876] [linear_slider_controller]: Got request to cancel goal
[move_group-4] [INFO] [1723589518.272153542] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status TIMED_OUT ...
[move_group-4] [INFO] [1723589518.272676190] [moveit_move_group_default_capabilities.move_action_capability]: Timeout reached
[rviz2-6] [INFO] [1723589518.274159933] [move_group_interface]: Plan and Execute request aborted
[rviz2-6] [ERROR] [1723589518.274772835] [move_group_interface]: MoveGroupInterface::move() failed or timeout reached
As detailed, the controller accepts the goal from MoveIt, but the controller completely times out. I used to be able to test the controller using rqt_joint_trajectory_controller, but this has also broken.
The files detailed below can also be found on GitHub here for better viewing. Much of the content was modeled after the Universal Robots ros2 drivers.
Disclaimer: I know this is a complicated question with many parts involved. Please let me know if you need more specific information in answering this question. I left out the URDF and base launch file to save on characters. I appreciate any help!
SRDF
The SRDF, generated using the same parameters as the URDF.
<?xml version="1.0" ?>
<robot name="linear_slider">
<group name="linear_slider__manipulator">
<chain base_link="linear_slider__base_link" tip_link="linear_slider__flange"/>
<end_effector group="linear_slider__manipulator" name="linear_slider__eef" parent_link="linear_slider__tool0"/>
</group>
<group_state group="linear_slider__manipulator" name="linear_slider__center">
<joint name="linear_slider__joint1" value="0.0"/>
</group_state>
<virtual_joint child_link="linear_slider__base" name="linear_slider__base_joint" parent_frame="world" type="fixed"/>
<disable_collisions link1="linear_slider__base_link" link2="linear_slider__moving_base" reason="Adjacent"/>
</robot>
ros2_controllers.yaml
I am using a velocity-commanded joint_trajectory_controller implementation. Here is my ros2_controllers.yaml file:
controller_manager:
ros__parameters:
update_rate: 2000 # hz
linear_slider_controller:
type: joint_trajectory_controller/JointTrajectoryController
# TODO: Therefore if the hardware provides only acceleration or velocity states they have to be integrated in the hardware-interface implementation of velocity and position to use these controllers.
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
limit_switch_state_broadcaster:
type: linear_slider_controllers/LimitSwitchStateBroadcaster
linear_slider_controller:
ros__parameters:
joints:
- $(var prefix)joint1
command_interfaces:
- velocity
state_interfaces:
- position
- velocity
gains: # required for velocity interface
$(var prefix)joint1:
ff_velocity_scale: 0.0
p: 6.0
i: 2.0
d: 2.0
# p: 100.0
# i: 2.0
# d: 5.0
# i_clamp: 1.0
state_publish_rate: 100.0 # defaults to 50
action_monitor_rate: 50.0 # defaults to 20
allow_partial_joints_goal: false # defaults to false
# stop_trajectory_duration: 0.1
constraints:
goal_time: 20.0 # defaults to 0.0 (start immediately)
stopped_velocity_tolerance: 0.01 # Defaults to 0.01
$(var prefix)joint1:
goal: 0.05 # position tolerace (m)
# trajectory: 0.20
allow_nonzero_velocity_at_trajectory_end: false
limit_switch_state_broadcaster:
ros__parameters:
state_interfaces:
- $(var prefix)lim_switch_neg/state
- $(var prefix)lim_switch_pos/state
joint_state_broadcaster:
ros__parameters:
joints:
- $(var prefix)joint1
moveit_controllers.yaml
The corresponding moveit_controllers file is below. Use of the MoveItSimpleControllerManager is detailed in the linear_slider_moveit.launch.py.
controller_names:
- scaled_joint_trajectory_controller
- linear_slider_controller
scaled_joint_trajectory_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: false
joints:
- $(var prefix)joint1
linear_slider_controller:
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- $(var prefix)joint1
MoveIt launch file
from launch import LaunchDescription, LaunchContext
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit, OnProcessStart
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution
from launch.utilities import perform_substitutions
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterFile
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from linear_slider_moveit_config.launch_common import load_yaml
from moveit_configs_utils import MoveItConfigsBuilder
import rclpy
import rclpy.logging
import os
import yaml
import json
logger = rclpy.logging.get_logger("linear_slider_moveit_config.launch_logger")
def launch_setup(context: LaunchContext, *args, **kwargs):
"""Callback function for launch setup using runtime context for evaluation and debugging."""
# Launch configurations
launch_rviz = LaunchConfiguration("launch_rviz")
launch_servo = LaunchConfiguration("launch_servo")
description_file = LaunchConfiguration("description_file")
description_pkg = LaunchConfiguration("description_pkg")
mock_sensor_commands = LaunchConfiguration("mock_sensor_commands")
moveit_semantic_description_file = LaunchConfiguration("moveit_semantic_description_file")
moveit_runtime_config_pkg = LaunchConfiguration("moveit_runtime_config_pkg")
moveit_joint_limits_file = LaunchConfiguration("moveit_joint_limits_file")
prefix = LaunchConfiguration("prefix")
use_mock_hardware = LaunchConfiguration("use_mock_hardware")
use_sim_time = LaunchConfiguration("use_sim_time")
warehouse_sqlite_path = LaunchConfiguration("warehouse_sqlite_path")
# Get URDF from xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare(description_pkg), "urdf", description_file]),
" ",
"prefix:=",
prefix,
" ",
"use_mock_hardware:=",
use_mock_hardware,
" ",
"mock_sensor_commands:=",
mock_sensor_commands,
" ",
]
)
robot_description = {"robot_description": robot_description_content}
# Get SRDF from xacro
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare(moveit_runtime_config_pkg), "srdf", moveit_semantic_description_file]),
" ",
"prefix:=",
prefix,
" ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
# MoveIt Kinematics
robot_description_kinematics_path = PathJoinSubstitution(
[FindPackageShare(moveit_runtime_config_pkg), "config", "kinematics.yaml"]
)
robot_description_kinematics_evaluated_file = ParameterFile(robot_description_kinematics_path, allow_substs=True)
robot_description_kinematics_evaluated_file.evaluate(context=context)
robot_description_kinematics_content = load_yaml(
package_name=str(moveit_runtime_config_pkg.perform(context=context)),
file_path=os.path.join("config", str(robot_description_kinematics_evaluated_file.param_file)),
)
kinematics_tmp_path = PathJoinSubstitution(
[ # For some reason, the temp file gets deleted pretty quickly, so rewrite the contents of that temp file to a slightly-more-permanent temp file in /linear_slider_moveit/config/tmp/
FindPackageShare(moveit_runtime_config_pkg),
"config",
"tmp",
"tmp_kinematics.yaml",
]
)
_sv_path = kinematics_tmp_path.perform(context=context)
with open(_sv_path, "w") as file:
yaml.safe_dump(robot_description_kinematics_content, file)
# MoveIt Joint Limits
moveit_joint_limits_path = PathJoinSubstitution(
[FindPackageShare(moveit_runtime_config_pkg), "config", moveit_joint_limits_file]
)
moveit_joint_limits_evaluated_file = ParameterFile(moveit_joint_limits_path, allow_substs=True)
moveit_joint_limits_evaluated_file.evaluate(context=context)
robot_description_planning = {
"robot_description_planning": load_yaml(
package_name=str(moveit_runtime_config_pkg.perform(context=context)),
file_path=os.path.join(
"config", str(moveit_joint_limits_evaluated_file.param_file)
), # .param_file gets name of temp yaml file created by ROS2
)
}
# Planning configuration
ompl_planning_pipeline_config = dict(
move_group=dict(
planning_plugins=["ompl_interface/OMPLPlanner"],
request_adapters= "default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/AddRuckigTrajectorySmoothing default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixStartStatePathConstraints default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/Empty",
response_adapters=[
"default_planning_response_adapters/AddTimeOptimalParameterization",
"default_planning_response_adapters/ValidateSolution",
"default_planning_response_adapters/DisplayMotionPath",
],
)
)
ompl_planning_content = load_yaml(
package_name=str(moveit_runtime_config_pkg.perform(context=context)),
file_path=os.path.join("config", "ompl_planning.yaml"),
)
ompl_planning_pipeline_config["move_group"].update(ompl_planning_content)
# Trajectory execution configuration
controllers_file = PathJoinSubstitution([FindPackageShare(moveit_runtime_config_pkg), "config", "controllers.yaml"])
controllers_yaml_evaluated_file = ParameterFile(controllers_file, allow_substs=True)
controllers_yaml_evaluated_file.evaluate(context=context)
controllers_content = load_yaml(
package_name=str(moveit_runtime_config_pkg.perform(context=context)),
file_path=os.path.join("config", str(controllers_yaml_evaluated_file.param_file)),
)
# The scaled_joint_trajectory_controller does not work on mock_hardware, switch to regular joint_trajectory_controller
change_controllers = context.perform_substitution(use_mock_hardware)
if change_controllers.lower() == "true":
controllers_content["scaled_joint_trajectory_controller"]["default"] = False
controllers_content["linear_slider_controller"]["default"] = True
moveit_controllers_content = dict(
moveit_simple_controller_manager=controllers_content,
moveit_controller_manager="moveit_simple_controller_manager/MoveItSimpleControllerManager",
)
""" The following parameters are used to compute the allowed trajectory execution duration by scaling the expected execution duration and adding the margin afterwards. If this duration is exceeded the trajectory will be cancelled. The controller-specific parameters can be set as follows TODO: put this in a yaml file..."""
trajectory_execution = {
"moveit_manage_controllers": False,
"trajectory_execution.allowed_execution_during_scaling": 10.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = dict(
publish_planning_scene=True,
publish_geometry_updates=True,
publish_state_updates=True,
publish_transform_updates=True,
)
warehouse_ros_config = dict(
warehouse_plugin="warehouse_ros_sqlite::DatabaseConnection", warehouse_host=warehouse_sqlite_path
)
mcb = MoveItConfigsBuilder(robot_name="linear_slider", package_name="linear_slider_moveit_config")
mcb.robot_description(
file_path=os.path.join(get_package_share_directory("linear_slider_description"), "urdf/linear_slider.urdf.xacro"),
mappings=robot_description
)
mcb.robot_description_semantic(
file_path=os.path.join(get_package_share_directory("linear_slider_moveit_config"), "srdf/linear_slider.srdf.xacro"),
mappings=robot_description_semantic
)
mcb.robot_description_kinematics(
file_path=os.path.join(get_package_share_directory("linear_slider_moveit_config"), "config/tmp/tmp_kinematics.yaml"),
)
mcb.planning_pipelines(
default_planning_pipeline="ompl",
pipelines=[
"ompl", "pilz_industrial_motion_planner", "chomp"
]
)
moveit_config = mcb.to_moveit_configs()
# logger.info(f"{robot_description_kinematics_content}")
# logger.warn(f"{moveit_config.robot_description_kinematics}")
# logger.info(f"{robot_description_semantic_content.perform(context=context)}")
# logger.warn(f"{moveit_config.robot_description_semantic['robot_description_semantic']}")
# Start the actual move_group node/action server
node_move_group = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
robot_description,
robot_description_semantic,
# robot_description_kinematics_path,
moveit_config.robot_description_kinematics,
robot_description_planning,
ompl_planning_pipeline_config,
trajectory_execution,
moveit_controllers_content,
planning_scene_monitor_parameters,
{"use_sim_time": use_sim_time},
warehouse_ros_config,
],
)
# RViz
rviz_config_file = PathJoinSubstitution(
[get_package_share_directory("linear_slider_moveit_config"), "rviz", "linear_slider_moveit.rviz"]
)
node_rviz = Node(
package="rviz2",
executable="rviz2",
name="rviz2_moveit",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
moveit_config.robot_description_kinematics,
robot_description_planning,
ompl_planning_pipeline_config,
warehouse_ros_config,
],
condition=IfCondition(launch_rviz),
)
# MoveIt Servo
servo_yaml_path = PathJoinSubstitution(
[FindPackageShare(moveit_runtime_config_pkg), "config", "linear_slider_servo.yaml"]
)
servo_yaml_evaluated_file = ParameterFile(servo_yaml_path, allow_substs=True)
servo_yaml_evaluated_file.evaluate(context=context)
servo_yaml_content = load_yaml(
package_name=str(moveit_runtime_config_pkg.perform(context=context)),
file_path=os.path.join("config", str(servo_yaml_evaluated_file.param_file)),
)
# logger.warn(f"{servo_yaml_content}")
servo_params = dict(moveit_servo=servo_yaml_content)
node_servo = Node(
package="moveit_servo",
executable="servo_node_main",
output="screen",
parameters=[servo_params, robot_description, robot_description_semantic, moveit_config.robot_description_kinematics],
condition=IfCondition(launch_servo)
)
nodes_to_start = [node_move_group, node_servo, node_rviz]
return nodes_to_start
def generate_launch_description():
declared_args = []
declared_args.append(
DeclareLaunchArgument(
"description_file",
default_value="linear_slider.urdf.xacro",
description="URDF/xacro description file of the robot.",
)
)
declared_args.append(
DeclareLaunchArgument(
"description_pkg",
default_value="linear_slider_description",
description="Description package with the robot URDF/xacro files. Usually, the argument is not set; it enables the use of a custom setup.",
)
)
declared_args.append(
DeclareLaunchArgument(
"launch_rviz",
default_value="true",
description="Launch RViz window."
)
)
declared_args.append(
DeclareLaunchArgument(
"launch_servo",
default_value="true",
description="Launch servoing node."
)
)
declared_args.append(
DeclareLaunchArgument(
"mock_sensor_commands",
default_value="false",
choices=["true", "false"],
description="Enable fake command interfaces for sensors for simple simulation. Use only if `use_mock_hardware` parameter is true.",
)
)
declared_args.append(
DeclareLaunchArgument(
"moveit_semantic_description_file",
default_value="linear_slider.srdf.xacro", # TODO: Get srdf.xacro like UR drivers
description="MoveIt SRDF/xacro description file with the robot.",
)
)
declared_args.append(
DeclareLaunchArgument(
"moveit_runtime_config_pkg",
default_value="linear_slider_moveit_config",
description="MoveIt config package with robot SRDF/xacro files. Usually, the argument is not set; it enables the use of a custom setup.",
)
)
declared_args.append(
DeclareLaunchArgument(
"moveit_joint_limits_file",
default_value="joint_limits.yaml",
description="MoveIt joint limits that augment or override the values from the URDF robot description.",
)
)
declared_args.append(
DeclareLaunchArgument(
"prefix",
default_value="linear_slider__",
description="Prefix of the joint names. Useful for multi-robot setup. If changed, joint names in the controllers' configuration need to be updated, or dynamically configured in the launch file.",
)
)
declared_args.append(
DeclareLaunchArgument(
"use_mock_hardware",
default_value="false",
choices=["true", "false"],
description="Start robot with fake hardware mirroring command to its states.",
)
)
declared_args.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="false",
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
)
)
declared_args.append(
DeclareLaunchArgument(
"warehouse_sqlite_path",
default_value=os.path.expanduser("~/.ros/warehouse_ros.sqlite"),
description="Path to where the warehouse database should be stored.",
)
)
ld = LaunchDescription(declared_args + [OpaqueFunction(function=launch_setup)])
return ld
