From 58abc9fa9c497d3899ad0c61a12effc020a90ed6 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 6 Jul 2024 19:56:56 -0400 Subject: [PATCH 01/12] add moveit planing into gz --- src/ovis_bringup/config/ovis_controllers.yaml | 27 ----- src/ovis_bringup/launch/sim.launch.py | 103 +++++++++++++++--- .../config/default_bridge.yaml | 9 +- src/ovis_description/urdf/gazebo.urdf.xacro | 11 +- .../urdf/ovis2/urdf/gazebo.urdf.xacro | 29 ----- .../urdf/ovis_standalone.urdf.xacro | 18 ++- .../config/ovis.ros2_control.xacro | 6 +- 7 files changed, 123 insertions(+), 80 deletions(-) delete mode 100644 src/ovis_bringup/config/ovis_controllers.yaml delete mode 100644 src/ovis_description/urdf/ovis2/urdf/gazebo.urdf.xacro diff --git a/src/ovis_bringup/config/ovis_controllers.yaml b/src/ovis_bringup/config/ovis_controllers.yaml deleted file mode 100644 index bc7266f..0000000 --- a/src/ovis_bringup/config/ovis_controllers.yaml +++ /dev/null @@ -1,27 +0,0 @@ -# This config file is used by ros2_control -controller_manager: - ros__parameters: - update_rate: 100 # Hz - - arm_controller: - # type: ovis_controller/OvisController - type: joint_trajectory_controller/JointTrajectoryController - - joint_state_broadcaster: - type: joint_state_broadcaster/JointStateBroadcaster - -arm_controller: - ros__parameters: - joints: - - ovis_joint_1 - - ovis_joint_2 - - ovis_joint_3 - - ovis_joint_4 - - ovis_joint_5 - - ovis_joint_6 - command_interfaces: - - position - # - velocity - state_interfaces: - - position - - velocity \ No newline at end of file diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 5b7301f..7293577 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -1,28 +1,72 @@ import os - -from ament_index_python.packages import get_package_share_directory +from ament_index_python import get_package_share_directory from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription - +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import Command + from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare from launch_ros.parameter_descriptions import ParameterValue -from launch.actions import DeclareLaunchArgument - def generate_launch_description(): - # Configure ROS nodes for launch - # Get the launch directory - pkg_ovis_bringup = get_package_share_directory('ovis_bringup') pkg_ovis_description = get_package_share_directory('ovis_description') + bringup_pkg_path = get_package_share_directory('ovis_bringup') + moveit_pkg_path = get_package_share_directory('ovis_moveit') pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + pkg_ovis_servo = get_package_share_directory('ovis_servo') + + # Get the URDF file (robot) + urdf_path = os.path.join(moveit_pkg_path, 'config', 'ovis.urdf.xacro') + robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) - # Get the URDF file + # Get the URDF file (world) world_file_name = 'worlds/base_world.world' world = os.path.join(pkg_ovis_description, world_file_name) + # Get the launch directory + pkg_ovis_moveit = get_package_share_directory('ovis_moveit') + + + # Include launch files based on the configuration + virtual_joints_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'static_virtual_joint_tfs.launch.py') + ) + ) + + # Takes the description and joint angles as inputs and publishes + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[ + {'robot_description': robot_desc}, + {"use_sim_time": True, } + ] + ) + + move_group_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'move_group.launch.py'), + ), + launch_arguments={ + "use_sim_time": "true", + }.items(), + ) + + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'moveit_rviz.launch.py') + ), + launch_arguments={ + "use_sim_time": "true", + }.items(), + ) + # Setup to launch the simulator and Gazebo world gz_sim = IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -56,16 +100,43 @@ def generate_launch_description(): output='screen' ) - # Include common launch configuration - common = IncludeLaunchDescription( + servo = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_ovis_bringup, "launch", "common.launch.py"), - ), + os.path.join(pkg_ovis_servo, 'launch', 'servo.launch.py') + ) ) + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster'], + output='screen' + ) + + load_arm_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'arm_controller'], + output='screen' + ) + + return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=create, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_arm_controller], + ) + ), + virtual_joints_launch, + robot_state_publisher, + move_group_launch, + rviz_launch, gz_sim, bridge, create, - #common, ]) diff --git a/src/ovis_description/config/default_bridge.yaml b/src/ovis_description/config/default_bridge.yaml index 54bc727..5fc953a 100644 --- a/src/ovis_description/config/default_bridge.yaml +++ b/src/ovis_description/config/default_bridge.yaml @@ -3,4 +3,11 @@ gz_topic_name: "clock" ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "gz.msgs.Clock" - direction: GZ_TO_ROS \ No newline at end of file + direction: GZ_TO_ROS + +# gz topic published by JointState plugin +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS diff --git a/src/ovis_description/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/gazebo.urdf.xacro index b5f1141..db724b2 100644 --- a/src/ovis_description/urdf/gazebo.urdf.xacro +++ b/src/ovis_description/urdf/gazebo.urdf.xacro @@ -2,13 +2,14 @@ - - - ${robot_namespace} - gazebo_ros_control/DefaultRobotHWSim - true + + robot_description + robot_state_publisher + $(find ovis_moveit)/config/ros2_controllers.yaml + controller_manager diff --git a/src/ovis_description/urdf/ovis2/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/ovis2/urdf/gazebo.urdf.xacro deleted file mode 100644 index b5f1141..0000000 --- a/src/ovis_description/urdf/ovis2/urdf/gazebo.urdf.xacro +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - ${robot_namespace} - gazebo_ros_control/DefaultRobotHWSim - true - - - - - - - - joint_states - - - ogre2 - - - \ No newline at end of file diff --git a/src/ovis_description/urdf/ovis_standalone.urdf.xacro b/src/ovis_description/urdf/ovis_standalone.urdf.xacro index c5a877b..9ecb600 100644 --- a/src/ovis_description/urdf/ovis_standalone.urdf.xacro +++ b/src/ovis_description/urdf/ovis_standalone.urdf.xacro @@ -11,7 +11,23 @@ - + + + + + + + + + + + + + + + + + diff --git a/src/ovis_moveit/config/ovis.ros2_control.xacro b/src/ovis_moveit/config/ovis.ros2_control.xacro index c4d6388..7f415bf 100644 --- a/src/ovis_moveit/config/ovis.ros2_control.xacro +++ b/src/ovis_moveit/config/ovis.ros2_control.xacro @@ -6,7 +6,11 @@ - mock_components/GenericSystem + gz_ros2_control/GazeboSimSystem + + + + From 7f7b2c1ab814f0f26a7946cb36ed57ea5f79c93d Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 6 Jul 2024 20:05:33 -0400 Subject: [PATCH 02/12] add vcs for ros2 control gz --- README.md | 1 + ovis.repos | 5 +++++ 2 files changed, 6 insertions(+) create mode 100644 ovis.repos diff --git a/README.md b/README.md index b739401..dc73a94 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,7 @@ Clone and build the repository : ```bash git clone https://github.com/clubcapra/ovis_ros2.git cd ovis_ros2 +vcs import src < ovis.repos colcon build --symlink-install source install/setup.bash ``` diff --git a/ovis.repos b/ovis.repos new file mode 100644 index 0000000..30b4951 --- /dev/null +++ b/ovis.repos @@ -0,0 +1,5 @@ +repositories: + gz_ros2_control: + type: git + url: https://github.com/clubcapra/gz_ros2_control.git + version: iron \ No newline at end of file From bf49cfe5ee35ffc388747a2a93eba790d3b68161 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 6 Jul 2024 20:37:18 -0400 Subject: [PATCH 03/12] force use sim tim on sim.launch.py --- src/ovis_bringup/launch/sim.launch.py | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 7293577..88d05d0 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -6,7 +6,7 @@ from launch.substitutions import Command, FindExecutable, PathJoinSubstitution from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node +from launch_ros.actions import Node, SetParameter from launch_ros.substitutions import FindPackageShare from launch_ros.parameter_descriptions import ParameterValue @@ -53,9 +53,13 @@ def generate_launch_description(): PythonLaunchDescriptionSource( os.path.join(pkg_ovis_moveit, 'launch', 'move_group.launch.py'), ), - launch_arguments={ - "use_sim_time": "true", - }.items(), + ) + + # Set the use_sim_time parameter to true for the entire system + # Move group doesn't accept use sim time as an argument, we need to manually call it or use SetParameter + set_use_sim_time = SetParameter( + name='use_sim_time', + value='true', ) rviz_launch = IncludeLaunchDescription( @@ -75,7 +79,7 @@ def generate_launch_description(): ) # Spawn robot - create = Node( + ovis_spawner = Node( package='ros_gz_sim', executable='create', arguments=['-name', 'ovis', @@ -122,7 +126,7 @@ def generate_launch_description(): return LaunchDescription([ RegisterEventHandler( event_handler=OnProcessExit( - target_action=create, + target_action=ovis_spawner, on_exit=[load_joint_state_broadcaster], ) ), @@ -134,9 +138,11 @@ def generate_launch_description(): ), virtual_joints_launch, robot_state_publisher, + set_use_sim_time, move_group_launch, rviz_launch, gz_sim, bridge, - create, + ovis_spawner, + servo, ]) From a16a6ff367e9dc1f9ce1e5a90b6b1d9835daf75a Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 6 Jul 2024 20:50:06 -0400 Subject: [PATCH 04/12] add vcs to docker image --- .devcontainer/Dockerfile | 32 ++++++++++++++++++++++++++++---- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 4606b4e..478b635 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -29,6 +29,20 @@ RUN apt update && apt upgrade -y # Install Git RUN apt install -y git +# Install necessary packages for Gazebo Harmonic and its dependencies +RUN apt-get update && apt-get install -y \ + software-properties-common \ + wget \ + lsb-release \ + gnupg \ + curl + +# Add Gazebo Harmonic repository +RUN wget https://packages.osrfoundation.org/gazebo.gpg -O /usr/share/keyrings/pkgs-osrf-archive-keyring.gpg +RUN echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/pkgs-osrf-archive-keyring.gpg] http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/gazebo-stable.list > /dev/null + +RUN apt update -y + # Change user RUN chown $USER_UID:$USER_GID /workspace/$USERNAME USER $USERNAME @@ -36,10 +50,20 @@ USER $USERNAME # Copy to preload the ros packages COPY ./ /workspace/$USERNAME/ +# Source the ROS setup file +RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc + +COPY --chown=$USERNAME ./ /workspace/$USERNAME + +# Add needed packages +RUN vcs import src < ovis.repos + + # Rosdep update RUN rosdep update -RUN rosdep install --from-paths src --ignore-src --rosdistro humble -y -# Source the ROS setup file -RUN echo "source /opt/ros/${ROS_DISTRO}/setup.bash" >> ~/.bashrc -RUN echo "source /gazebo/gazebo_ws/install/setup.bash" >> ~/.bashrc +# Install ROS dependencies +RUN rosdep install --from-paths src --ignore-src --rosdistro humble -y --skip-keys=gz-plugin2 --skip-keys=gz-sim8 + +# Install gzharmonic after rosdep install +RUN sudo apt install -y ros-humble-ros-gzharmonic \ No newline at end of file From b8ccfe12e97d399365e32704c1fef76470909dff Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 6 Jul 2024 22:16:04 -0400 Subject: [PATCH 05/12] add xacro if condition (gazebo, mock, real ovis) --- src/ovis_bringup/launch/combined.launch.py | 64 --------- src/ovis_bringup/launch/launch.py | 50 +------ src/ovis_bringup/launch/real.launch.py | 90 ++++++++++++ src/ovis_bringup/launch/sim.launch.py | 9 +- .../config/ovis.ros2_control.xacro | 134 +++++++++--------- src/ovis_moveit/config/ovis.urdf.xacro | 3 +- 6 files changed, 165 insertions(+), 185 deletions(-) delete mode 100644 src/ovis_bringup/launch/combined.launch.py create mode 100644 src/ovis_bringup/launch/real.launch.py diff --git a/src/ovis_bringup/launch/combined.launch.py b/src/ovis_bringup/launch/combined.launch.py deleted file mode 100644 index 0e93e69..0000000 --- a/src/ovis_bringup/launch/combined.launch.py +++ /dev/null @@ -1,64 +0,0 @@ -import os -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription, ExecuteProcess, TimerAction, RegisterEventHandler, LogInfo -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.event_handlers import OnProcessExit -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - # Paths to other launch files - demo_launch = os.path.join(get_package_share_directory('ovis_moveit'), 'launch', 'demo.launch.py') - servo_demo_launch = os.path.join(get_package_share_directory('ovis_servo'), 'launch', 'servo_demo.launch.py') - - # Node for publishing the initial trajectory - publish_trajectory_node = Node( - package='ovis_bringup', - executable='publish_trajectory.py', - name='publish_trajectory_node', - output='screen' - ) - - # Event handler to launch servo_keyboard_input after publish_trajectory_node exits - on_publish_trajectory_exit = RegisterEventHandler( - event_handler=OnProcessExit( - target_action=publish_trajectory_node, - on_exit=[ - LogInfo(msg="Starting servo_keyboard_input node..."), - ExecuteProcess( - cmd=['gnome-terminal', '--', 'ros2', 'run', 'ovis_realtime_servo', 'servo_keyboard_input'], - output='screen' - ) - ] - ) - ) - - return LaunchDescription([ - # Include the existing demo launch file - IncludeLaunchDescription( - PythonLaunchDescriptionSource(demo_launch) - ), - # Include the existing servo_demo launch file - IncludeLaunchDescription( - PythonLaunchDescriptionSource(servo_demo_launch) - ), - # Start the servo service - TimerAction( - period=5.0, - actions=[ - ExecuteProcess( - cmd=['ros2', 'service', 'call', '/servo_node/start_servo', 'std_srvs/srv/Trigger', '{}'], - output='screen' - ) - ] - ), - # Publish the initial trajectory - TimerAction( - period=10.0, - actions=[ - publish_trajectory_node - ] - ), - # Launch servo_keyboard_input after publish_trajectory_node exits - on_publish_trajectory_exit - ]) diff --git a/src/ovis_bringup/launch/launch.py b/src/ovis_bringup/launch/launch.py index 379ca01..afbab3e 100644 --- a/src/ovis_bringup/launch/launch.py +++ b/src/ovis_bringup/launch/launch.py @@ -1,31 +1,21 @@ import os from ament_index_python import get_package_share_directory from launch import LaunchDescription -from launch.actions import RegisterEventHandler -from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import Command from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): # Get the launch directory - pkg_ovis_description = get_package_share_directory('ovis_description') - bringup_pkg_path = get_package_share_directory('ovis_bringup') moveit_pkg_path = get_package_share_directory('ovis_moveit') - pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') pkg_ovis_servo = get_package_share_directory('ovis_servo') # Get the URDF file (robot) urdf_path = os.path.join(moveit_pkg_path, 'config', 'ovis.urdf.xacro') - robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) - - # Get the URDF file (world) - world_file_name = 'worlds/base_world.world' - world = os.path.join(pkg_ovis_description, world_file_name) + robot_desc = ParameterValue(Command(['xacro ', urdf_path, ' hardware_type:=', "mock"]), value_type=str) # Get the launch directory pkg_ovis_moveit = get_package_share_directory('ovis_moveit') @@ -83,39 +73,6 @@ def generate_launch_description(): ) ) - # Setup to launch the simulator and Gazebo world - gz_sim = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')), - launch_arguments={'gz_args': "-v 4 -r " + world}.items(), - ) - - # Spawn robot - create = Node( - package='ros_gz_sim', - executable='create', - arguments=['-name', 'ovis', - '-topic', 'robot_description', - '-x', '0', - '-y', '0', - '-z', '0.1', - ], - output='screen', - ) - - # Bridge ROS topics and Gazebo messages for establishing communication - bridge = Node( - package='ros_gz_bridge', - executable='parameter_bridge', - parameters=[{ - 'config_file': os.path.join(pkg_ovis_description, 'config', - 'default_bridge.yaml'), - 'qos_overrides./tf_static.publisher.durability': 'transient_local', - "use_sim_time": True, - }], - output='screen' - ) - servo = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_ovis_servo, 'launch', 'servo.launch.py') @@ -130,7 +87,4 @@ def generate_launch_description(): fake_joint_driver, spawn_controllers_launch, servo, - #gz_sim, - #bridge, - #create, ]) diff --git a/src/ovis_bringup/launch/real.launch.py b/src/ovis_bringup/launch/real.launch.py new file mode 100644 index 0000000..2cefbfa --- /dev/null +++ b/src/ovis_bringup/launch/real.launch.py @@ -0,0 +1,90 @@ +import os +from ament_index_python import get_package_share_directory +from launch import LaunchDescription +from launch.substitutions import Command +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import IncludeLaunchDescription + +from launch_ros.actions import Node +from launch_ros.parameter_descriptions import ParameterValue + +def generate_launch_description(): + # Get the launch directory + moveit_pkg_path = get_package_share_directory('ovis_moveit') + pkg_ovis_servo = get_package_share_directory('ovis_servo') + + # Get the URDF file (robot) + urdf_path = os.path.join(moveit_pkg_path, 'config', 'ovis.urdf.xacro') + robot_desc = ParameterValue(Command(['xacro ', urdf_path, ' hardware_type:=', "ovis"]), value_type=str) + + # Get the launch directory + pkg_ovis_moveit = get_package_share_directory('ovis_moveit') + + + # Include launch files based on the configuration + virtual_joints_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'static_virtual_joint_tfs.launch.py') + ) + ) + + # Takes the description and joint angles as inputs and publishes + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='both', + parameters=[ + {'robot_description': robot_desc}, + {"use_sim_time": True, } + ] + ) + + move_group_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'move_group.launch.py'), + ), + launch_arguments={ + "use_sim_time": "true", + }.items(), + ) + + rviz_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'moveit_rviz.launch.py') + ), + ) + + + # kinova driver + kinova_joint_driver = Node( + package='controller_manager', + executable='ros2_control_node', + parameters=[ + {'robot_description': robot_desc}, + os.path.join(pkg_ovis_moveit, 'config', 'ros2_controllers.yaml'), + + ], + ) + + spawn_controllers_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_moveit, 'launch', 'spawn_controllers.launch.py') + ) + ) + + servo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_ovis_servo, 'launch', 'servo.launch.py') + ) + ) + + return LaunchDescription([ + virtual_joints_launch, + robot_state_publisher, + move_group_launch, + rviz_launch, + kinova_joint_driver, + spawn_controllers_launch, + servo, + ]) diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 88d05d0..0fd1ca4 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -1,13 +1,12 @@ import os from ament_index_python import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler from launch.event_handlers import OnProcessExit -from launch.substitutions import Command, FindExecutable, PathJoinSubstitution +from launch.substitutions import Command from launch.launch_description_sources import PythonLaunchDescriptionSource from launch_ros.actions import Node, SetParameter -from launch_ros.substitutions import FindPackageShare from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): @@ -20,7 +19,7 @@ def generate_launch_description(): # Get the URDF file (robot) urdf_path = os.path.join(moveit_pkg_path, 'config', 'ovis.urdf.xacro') - robot_desc = ParameterValue(Command(['xacro ', urdf_path]), value_type=str) + robot_desc = ParameterValue(Command(['xacro ', urdf_path, ' hardware_type:=', "gazebo"]), value_type=str) # Get the URDF file (world) world_file_name = 'worlds/base_world.world' @@ -78,7 +77,7 @@ def generate_launch_description(): launch_arguments={'gz_args': "-v 4 -r " + world}.items(), ) - # Spawn robot + # Spawn ovis (robot arm) ovis_spawner = Node( package='ros_gz_sim', executable='create', diff --git a/src/ovis_moveit/config/ovis.ros2_control.xacro b/src/ovis_moveit/config/ovis.ros2_control.xacro index 7f415bf..d4fe7d3 100644 --- a/src/ovis_moveit/config/ovis.ros2_control.xacro +++ b/src/ovis_moveit/config/ovis.ros2_control.xacro @@ -1,69 +1,69 @@ - - - - - - - gz_ros2_control/GazeboSimSystem - - - - - - - - - - - - - ${initial_positions['ovis_joint_1']} - - - - - - - ${initial_positions['ovis_joint_2']} - - - - - - - ${initial_positions['ovis_joint_3']} - - - - - - - ${initial_positions['ovis_joint_4']} - - - - - - - ${initial_positions['ovis_joint_5']} - - - - - - - ${initial_positions['ovis_joint_6']} - - - - - - - + + + + + + gz_ros2_control/GazeboSimSystem + + + mock_components/GenericSystem + + + ovis_control/OvisHWInterface + ETHERNET + 192.168.84.15 + 255.255.255.0 + 25015 + 25025 + 192.168.84.160 + + + + + + + ${initial_positions['ovis_joint_1']} + + + + + + + ${initial_positions['ovis_joint_2']} + + + + + + + ${initial_positions['ovis_joint_3']} + + + + + + + ${initial_positions['ovis_joint_4']} + + + + + + + ${initial_positions['ovis_joint_5']} + + + + + + + ${initial_positions['ovis_joint_6']} + + + + + + \ No newline at end of file diff --git a/src/ovis_moveit/config/ovis.urdf.xacro b/src/ovis_moveit/config/ovis.urdf.xacro index 1deee86..2b71864 100644 --- a/src/ovis_moveit/config/ovis.urdf.xacro +++ b/src/ovis_moveit/config/ovis.urdf.xacro @@ -1,6 +1,7 @@ + @@ -9,6 +10,6 @@ - + From c999f9415934c9672c15f3e6d839de1ed3066925 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sat, 6 Jul 2024 23:58:24 -0400 Subject: [PATCH 06/12] remove need for gazebo_config --- src/ovis_bringup/launch/sim.launch.py | 11 +--------- src/ovis_description/urdf/gazebo.urdf.xacro | 22 +++++++++---------- src/ovis_description/urdf/ovis.urdf.xacro | 2 -- .../urdf/ovis2/urdf/ovis.urdf.xacro | 1 - 4 files changed, 11 insertions(+), 25 deletions(-) diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 0fd1ca4..fc975b8 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -28,14 +28,6 @@ def generate_launch_description(): # Get the launch directory pkg_ovis_moveit = get_package_share_directory('ovis_moveit') - - # Include launch files based on the configuration - virtual_joints_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ovis_moveit, 'launch', 'static_virtual_joint_tfs.launch.py') - ) - ) - # Takes the description and joint angles as inputs and publishes robot_state_publisher = Node( package='robot_state_publisher', @@ -44,7 +36,7 @@ def generate_launch_description(): output='both', parameters=[ {'robot_description': robot_desc}, - {"use_sim_time": True, } + {"use_sim_time": True } ] ) @@ -135,7 +127,6 @@ def generate_launch_description(): on_exit=[load_arm_controller], ) ), - virtual_joints_launch, robot_state_publisher, set_use_sim_time, move_group_launch, diff --git a/src/ovis_description/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/gazebo.urdf.xacro index db724b2..77a9349 100644 --- a/src/ovis_description/urdf/gazebo.urdf.xacro +++ b/src/ovis_description/urdf/gazebo.urdf.xacro @@ -1,19 +1,17 @@ - - - - - robot_description - robot_state_publisher - $(find ovis_moveit)/config/ros2_controllers.yaml - controller_manager - - + + + + robot_description + robot_state_publisher + $(find ovis_moveit)/config/ros2_controllers.yaml + controller_manager + + - - - - From b0633f336c787e861a736e8dd458e20dfb52c212 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 25 Aug 2024 10:50:19 -0400 Subject: [PATCH 07/12] test ovis namespace --- src/ovis_bringup/launch/sim.launch.py | 35 +-- .../config/default_bridge.yaml | 1 + src/ovis_description/urdf/gazebo.urdf.xacro | 6 + src/ovis_moveit/config/moveit.rviz | 232 +++++++++++++++++- .../config/moveit_controllers.yaml | 4 +- src/ovis_moveit/launch/move_group.launch.py | 36 ++- src/ovis_moveit/launch/moveit_rviz.launch.py | 23 +- src/ovis_servo/config/ovis_gazebo_config.yaml | 4 +- .../config/ovis_simulated_config.yaml | 28 +-- src/ovis_servo/launch/servo.launch.py | 5 +- 10 files changed, 328 insertions(+), 46 deletions(-) diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index fc975b8..96f6913 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -28,11 +28,14 @@ def generate_launch_description(): # Get the launch directory pkg_ovis_moveit = get_package_share_directory('ovis_moveit') + namespace = '/ovis' + # Takes the description and joint angles as inputs and publishes robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', name='robot_state_publisher', + namespace=namespace, output='both', parameters=[ {'robot_description': robot_desc}, @@ -72,9 +75,10 @@ def generate_launch_description(): # Spawn ovis (robot arm) ovis_spawner = Node( package='ros_gz_sim', + namespace=namespace, executable='create', arguments=['-name', 'ovis', - '-topic', 'robot_description', + '-topic', '/ovis/robot_description', '-x', '0', '-y', '0', '-z', '0.1', @@ -86,6 +90,7 @@ def generate_launch_description(): bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', + namespace=namespace, parameters=[{ 'config_file': os.path.join(pkg_ovis_description, 'config', 'default_bridge.yaml'), @@ -103,30 +108,30 @@ def generate_launch_description(): load_joint_state_broadcaster = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'joint_state_broadcaster'], + 'joint_state_broadcaster', '-c', '/ovis/controller_manager'], output='screen' ) load_arm_controller = ExecuteProcess( cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', - 'arm_controller'], + 'arm_controller', '-c', '/ovis/controller_manager'], output='screen' ) return LaunchDescription([ - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=ovis_spawner, - on_exit=[load_joint_state_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=load_joint_state_broadcaster, - on_exit=[load_arm_controller], - ) - ), + # RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=ovis_spawner, + # on_exit=[load_joint_state_broadcaster], + # ) + # ), + # RegisterEventHandler( + # event_handler=OnProcessExit( + # target_action=load_joint_state_broadcaster, + # on_exit=[load_arm_controller], + # ) + # ), robot_state_publisher, set_use_sim_time, move_group_launch, diff --git a/src/ovis_description/config/default_bridge.yaml b/src/ovis_description/config/default_bridge.yaml index 5fc953a..71a25b0 100644 --- a/src/ovis_description/config/default_bridge.yaml +++ b/src/ovis_description/config/default_bridge.yaml @@ -11,3 +11,4 @@ ros_type_name: "sensor_msgs/msg/JointState" gz_type_name: "gz.msgs.Model" direction: GZ_TO_ROS +# remove gz topic published by JointState plugin diff --git a/src/ovis_description/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/gazebo.urdf.xacro index 77a9349..0ea9b7d 100644 --- a/src/ovis_description/urdf/gazebo.urdf.xacro +++ b/src/ovis_description/urdf/gazebo.urdf.xacro @@ -9,6 +9,9 @@ robot_state_publisher $(find ovis_moveit)/config/ros2_controllers.yaml controller_manager + + /ovis + @@ -18,6 +21,9 @@ filename="gz-sim-joint-state-publisher-system" name="gz::sim::systems::JointStatePublisher"> joint_states + + /ovis + Value: true - - Class: moveit_rviz_plugin/MotionPlanning + - Acceleration_Scaling_Factor: 0.1 + Class: moveit_rviz_plugin/MotionPlanning + Enabled: true + Move Group Namespace: "ovis" + MoveIt_Allow_Approximate_IK: false + MoveIt_Allow_External_Program: false + MoveIt_Allow_Replanning: false + MoveIt_Allow_Sensor_Positioning: false + MoveIt_Planning_Attempts: 10 + MoveIt_Planning_Time: 5 + MoveIt_Use_Cartesian_Path: false + MoveIt_Use_Constraint_Aware_IK: false + MoveIt_Workspace: + Center: + X: 0 + Y: 0 + Z: 0 + Size: + X: 2 + Y: 2 + Z: 2 Name: MotionPlanning Planned Path: + Color Enabled: false + Interrupt Display: false + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_end_effector: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Loop Animation: true + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Show Robot Collision: false + Show Robot Visual: true + Show Trail: false State Display Time: 0.05 s + Trail Step Size: 1 Trajectory Topic: display_planned_path + Use Sim Time: false + Planning Metrics: + Payload: 1 + Show Joint Torques: false + Show Manipulability: false + Show Manipulability Index: false + Show Weight Limit: false + TextHeight: 0.07999999821186066 + Planning Request: + Colliding Link Color: 255; 0; 0 + Goal State Alpha: 1 + Goal State Color: 250; 128; 0 + Interactive Marker Size: 0 + Joint Violation Color: 255; 0; 255 + Planning Group: arm + Query Goal State: true + Query Start State: false + Show Workspace: false + Start State Alpha: 1 + Start State Color: 0; 255; 0 Planning Scene Topic: monitored_planning_scene Robot Description: robot_description Scene Geometry: Scene Alpha: 1 + Scene Color: 50; 230; 50 + Scene Display Time: 0.009999999776482582 + Show Scene Geometry: true + Voxel Coloring: Z-Axis + Voxel Rendering: Occupied Voxels Scene Robot: + Attached Body Color: 150; 50; 150 + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_end_effector: + Alpha: 1 + Show Axes: false + Show Trail: false + ovis_link_1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + ovis_link_base: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false Robot Alpha: 0.5 + Show Robot Collision: false + Show Robot Visual: true Value: true + Velocity_Scaling_Factor: 0.1 + Enabled: true Global Options: + Background Color: 48; 48; 48 Fixed Frame: 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 + Transformation: + Current: + Class: rviz_default_plugins/TF Value: true Views: Current: Class: rviz_default_plugins/Orbit - Distance: 2.0 + Distance: 5.233532905578613 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false Focal Point: - X: -0.1 + X: -0.10000000149011612 Y: 0.25 - Z: 0.30 + Z: 0.30000001192092896 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false Name: Current View - Pitch: 0.5 + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5949999690055847 Target Frame: base_link - Yaw: -0.623 + Value: Orbit (rviz_default_plugins) + Yaw: 4.385163307189941 + Saved: ~ Window Geometry: + Displays: + collapsed: false Height: 975 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Views: + collapsed: false Width: 1200 + X: 1970 + Y: 0 diff --git a/src/ovis_moveit/config/moveit_controllers.yaml b/src/ovis_moveit/config/moveit_controllers.yaml index 0d720d4..5d8a6bc 100644 --- a/src/ovis_moveit/config/moveit_controllers.yaml +++ b/src/ovis_moveit/config/moveit_controllers.yaml @@ -16,6 +16,4 @@ moveit_simple_controller_manager: - ovis_joint_3 - ovis_joint_4 - ovis_joint_5 - - ovis_joint_6 - action_ns: follow_joint_trajectory - default: true \ No newline at end of file + - ovis_joint_6 \ No newline at end of file diff --git a/src/ovis_moveit/launch/move_group.launch.py b/src/ovis_moveit/launch/move_group.launch.py index f17aa14..ca80e3d 100644 --- a/src/ovis_moveit/launch/move_group.launch.py +++ b/src/ovis_moveit/launch/move_group.launch.py @@ -1,7 +1,39 @@ from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_move_group_launch - +from launch import LaunchDescription +from launch_ros.actions import Node, SetParameter +from launch_ros.parameter_descriptions import ParameterValue def generate_launch_description(): moveit_config = MoveItConfigsBuilder("ovis", package_name="ovis_moveit").to_moveit_configs() - return generate_move_group_launch(moveit_config) + + move_group_configuration = { + "publish_robot_description_semantic": True, + "allow_trajectory_execution": True, + # Note: Wrapping the following values is necessary so that the parameter value can be the empty string + "capabilities": ParameterValue(""), + "disable_capabilities": ParameterValue(""), + # Publish the planning scene of the physical robot so that rviz plugin can know actual robot + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + "robot_description_param" : '/ovis/robot_description', + "monitor_dynamics": False, + } + + move_group_params = [ + moveit_config.to_dict(), + move_group_configuration, + ] + + move_group = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + namespace="ovis", + parameters=move_group_params, + # Set the display variable, in case OpenGL code is used internally + additional_env={"DISPLAY": ":0"}, + ) + return LaunchDescription([move_group]) \ No newline at end of file diff --git a/src/ovis_moveit/launch/moveit_rviz.launch.py b/src/ovis_moveit/launch/moveit_rviz.launch.py index 6461e2e..8e3162c 100644 --- a/src/ovis_moveit/launch/moveit_rviz.launch.py +++ b/src/ovis_moveit/launch/moveit_rviz.launch.py @@ -1,7 +1,26 @@ +from ast import arg from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_moveit_rviz_launch - +from launch import LaunchDescription +from launch_ros.actions import Node, SetParameter def generate_launch_description(): moveit_config = MoveItConfigsBuilder("ovis", package_name="ovis_moveit").to_moveit_configs() - return generate_moveit_rviz_launch(moveit_config) + + + rviz_parameters = [ + moveit_config.planning_pipelines, + moveit_config.robot_description_kinematics, + ] + + moveit_rviz = Node( + package="rviz2", + executable="rviz2", + namespace="ovis", + output="log", + respawn=False, + arguments=["-d", str(moveit_config.package_path / "config/moveit.rviz")], + parameters=rviz_parameters, + ) + + return LaunchDescription([moveit_rviz]) \ No newline at end of file diff --git a/src/ovis_servo/config/ovis_gazebo_config.yaml b/src/ovis_servo/config/ovis_gazebo_config.yaml index ff0d429..6f3388b 100644 --- a/src/ovis_servo/config/ovis_gazebo_config.yaml +++ b/src/ovis_servo/config/ovis_gazebo_config.yaml @@ -60,9 +60,9 @@ leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: /joint_states +joint_topic: /ovis/joint_states status_topic: ~/status # Publish status to this topic -command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here +command_out_topic: /ovis/arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/src/ovis_servo/config/ovis_simulated_config.yaml b/src/ovis_servo/config/ovis_simulated_config.yaml index 4db8da9..7773156 100644 --- a/src/ovis_servo/config/ovis_simulated_config.yaml +++ b/src/ovis_servo/config/ovis_simulated_config.yaml @@ -1,14 +1,14 @@ ############################################### # Modify all parameters related to servoing here ############################################### -use_gazebo: false # Whether the robot is started in a Gazebo simulation environment +use_gazebo: true # Whether the robot is started in a Gazebo simulation environment ## Properties of incoming commands command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.6 @@ -16,8 +16,8 @@ scale: # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] ## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] -low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) +publish_period: 0.034 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory @@ -38,31 +38,31 @@ smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" is_primary_planning_scene_monitor: true ## MoveIt properties -move_group_name: arm # Often 'manipulator' or 'arm' -planning_frame: ovis_link_base # The MoveIt planning frame. Often 'base_link' or 'world' +move_group_name: arm # Often 'manipulator' or 'arm' +planning_frame: ovis_link_base # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: ovis_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: ovis_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits -lower_singularity_threshold: 50.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 50.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 75.0 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: /joint_states +joint_topic: ~/joint_states status_topic: ~/status # Publish status to this topic -command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here +command_out_topic: ~/arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/src/ovis_servo/launch/servo.launch.py b/src/ovis_servo/launch/servo.launch.py index 8de0372..f62fe87 100644 --- a/src/ovis_servo/launch/servo.launch.py +++ b/src/ovis_servo/launch/servo.launch.py @@ -53,17 +53,19 @@ def generate_launch_description(): # Launch as much as possible in components container = ComposableNodeContainer( name="moveit_servo_demo_container", - namespace="/", + namespace="ovis", package="rclcpp_components", executable="component_container_mt", composable_node_descriptions=[ ComposableNode( package="ovis_servo", + namespace="ovis", plugin="ovis_servo::JoyToServoPub", name="controller_to_servo_node", ), ComposableNode( package="joy", + namespace="ovis", plugin="joy::Joy", name="joy_node", ), @@ -75,6 +77,7 @@ def generate_launch_description(): servo_node = Node( package="moveit_servo", executable="servo_node_main", + namespace="ovis", parameters=[ servo_params, kine_params, From e6286d4e5a2fe7a7178b66afc5ad5e40d1c888ab Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 25 Aug 2024 14:18:19 -0400 Subject: [PATCH 08/12] fix errors and add controller namespace --- src/ovis_bringup/launch/sim.launch.py | 27 +++++----- src/ovis_description/urdf/gazebo.urdf.xacro | 4 +- src/ovis_description/urdf/kinova_common.xacro | 2 +- .../urdf/ovis2/urdf/kinova_common.xacro | 2 +- .../urdf/ovis2/urdf/ovis_common.xacro | 2 +- src/ovis_moveit/config/moveit.rviz | 49 +------------------ .../config/moveit_controllers.yaml | 6 +-- src/ovis_moveit/config/ros2_controllers.yaml | 9 ++-- src/ovis_moveit/launch/move_group.launch.py | 1 - .../config/ovis_simulated_config.yaml | 4 +- src/ovis_servo/launch/servo.launch.py | 4 +- 11 files changed, 30 insertions(+), 80 deletions(-) diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 96f6913..8c448f2 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -90,7 +90,6 @@ def generate_launch_description(): bridge = Node( package='ros_gz_bridge', executable='parameter_bridge', - namespace=namespace, parameters=[{ 'config_file': os.path.join(pkg_ovis_description, 'config', 'default_bridge.yaml'), @@ -120,22 +119,22 @@ def generate_launch_description(): return LaunchDescription([ - # RegisterEventHandler( - # event_handler=OnProcessExit( - # target_action=ovis_spawner, - # on_exit=[load_joint_state_broadcaster], - # ) - # ), - # RegisterEventHandler( - # event_handler=OnProcessExit( - # target_action=load_joint_state_broadcaster, - # on_exit=[load_arm_controller], - # ) - # ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=ovis_spawner, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_arm_controller], + ) + ), robot_state_publisher, set_use_sim_time, move_group_launch, - rviz_launch, + # rviz_launch, gz_sim, bridge, ovis_spawner, diff --git a/src/ovis_description/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/gazebo.urdf.xacro index 0ea9b7d..3e44858 100644 --- a/src/ovis_description/urdf/gazebo.urdf.xacro +++ b/src/ovis_description/urdf/gazebo.urdf.xacro @@ -8,7 +8,7 @@ robot_description robot_state_publisher $(find ovis_moveit)/config/ros2_controllers.yaml - controller_manager + /ovis/controller_manager /ovis @@ -25,7 +25,7 @@ /ovis - ogre2 diff --git a/src/ovis_description/urdf/kinova_common.xacro b/src/ovis_description/urdf/kinova_common.xacro index 9cce001..e60846f 100644 --- a/src/ovis_description/urdf/kinova_common.xacro +++ b/src/ovis_description/urdf/kinova_common.xacro @@ -112,7 +112,7 @@ true - + 30.0 ${joint_name}_ft_sensor_topic ${joint_name} diff --git a/src/ovis_description/urdf/ovis2/urdf/kinova_common.xacro b/src/ovis_description/urdf/ovis2/urdf/kinova_common.xacro index 9cce001..e60846f 100644 --- a/src/ovis_description/urdf/ovis2/urdf/kinova_common.xacro +++ b/src/ovis_description/urdf/ovis2/urdf/kinova_common.xacro @@ -112,7 +112,7 @@ true - + 30.0 ${joint_name}_ft_sensor_topic ${joint_name} diff --git a/src/ovis_description/urdf/ovis2/urdf/ovis_common.xacro b/src/ovis_description/urdf/ovis2/urdf/ovis_common.xacro index 19933cd..51d6ec5 100644 --- a/src/ovis_description/urdf/ovis2/urdf/ovis_common.xacro +++ b/src/ovis_description/urdf/ovis2/urdf/ovis_common.xacro @@ -103,7 +103,7 @@ true - + 30.0 ${joint_name}_ft_sensor_topic ${joint_name} diff --git a/src/ovis_moveit/config/moveit.rviz b/src/ovis_moveit/config/moveit.rviz index 1a42a95..3d4f8d7 100644 --- a/src/ovis_moveit/config/moveit.rviz +++ b/src/ovis_moveit/config/moveit.rviz @@ -38,7 +38,7 @@ Visualization Manager: - Acceleration_Scaling_Factor: 0.1 Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "ovis" + Move Group Namespace: "" MoveIt_Allow_Approximate_IK: false MoveIt_Allow_External_Program: false MoveIt_Allow_Replanning: false @@ -159,53 +159,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - ovis_end_effector: - Alpha: 1 - Show Axes: false - Show Trail: false - ovis_link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false Robot Alpha: 0.5 Show Robot Collision: false Show Robot Visual: true diff --git a/src/ovis_moveit/config/moveit_controllers.yaml b/src/ovis_moveit/config/moveit_controllers.yaml index 5d8a6bc..793092e 100644 --- a/src/ovis_moveit/config/moveit_controllers.yaml +++ b/src/ovis_moveit/config/moveit_controllers.yaml @@ -4,9 +4,9 @@ moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControll moveit_simple_controller_manager: controller_names: - - arm_controller + - /ovis/arm_controller - arm_controller: + /ovis/arm_controller: type: FollowJointTrajectory action_ns: follow_joint_trajectory default: true @@ -16,4 +16,4 @@ moveit_simple_controller_manager: - ovis_joint_3 - ovis_joint_4 - ovis_joint_5 - - ovis_joint_6 \ No newline at end of file + - ovis_joint_6 diff --git a/src/ovis_moveit/config/ros2_controllers.yaml b/src/ovis_moveit/config/ros2_controllers.yaml index 983708c..a767eb3 100644 --- a/src/ovis_moveit/config/ros2_controllers.yaml +++ b/src/ovis_moveit/config/ros2_controllers.yaml @@ -1,16 +1,15 @@ # This config file is used by ros2_control -controller_manager: +/ovis/controller_manager: ros__parameters: - update_rate: 100 # Hz + update_rate: 100 # Hz arm_controller: type: joint_trajectory_controller/JointTrajectoryController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster -arm_controller: +/ovis/arm_controller: ros__parameters: joints: - ovis_joint_1 @@ -23,4 +22,4 @@ arm_controller: - position state_interfaces: - position - - velocity \ No newline at end of file + - velocity diff --git a/src/ovis_moveit/launch/move_group.launch.py b/src/ovis_moveit/launch/move_group.launch.py index ca80e3d..8485322 100644 --- a/src/ovis_moveit/launch/move_group.launch.py +++ b/src/ovis_moveit/launch/move_group.launch.py @@ -18,7 +18,6 @@ def generate_launch_description(): "publish_geometry_updates": True, "publish_state_updates": True, "publish_transforms_updates": True, - "robot_description_param" : '/ovis/robot_description', "monitor_dynamics": False, } diff --git a/src/ovis_servo/config/ovis_simulated_config.yaml b/src/ovis_servo/config/ovis_simulated_config.yaml index 7773156..bfaa63a 100644 --- a/src/ovis_servo/config/ovis_simulated_config.yaml +++ b/src/ovis_servo/config/ovis_simulated_config.yaml @@ -60,9 +60,9 @@ leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: ~/joint_states +joint_topic: /ovis/joint_states status_topic: ~/status # Publish status to this topic -command_out_topic: ~/arm_controller/joint_trajectory # Publish outgoing commands here +command_out_topic: /ovis/arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? diff --git a/src/ovis_servo/launch/servo.launch.py b/src/ovis_servo/launch/servo.launch.py index f62fe87..5eb206c 100644 --- a/src/ovis_servo/launch/servo.launch.py +++ b/src/ovis_servo/launch/servo.launch.py @@ -59,15 +59,15 @@ def generate_launch_description(): composable_node_descriptions=[ ComposableNode( package="ovis_servo", - namespace="ovis", plugin="ovis_servo::JoyToServoPub", name="controller_to_servo_node", + parameters=[{'use_sim_time': True}], ), ComposableNode( package="joy", - namespace="ovis", plugin="joy::Joy", name="joy_node", + parameters=[{'use_sim_time': True}], ), ], output="screen", From 80a71ae50eae0827d98891b1d2d5b58578ca39ec Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Sun, 25 Aug 2024 15:57:07 -0400 Subject: [PATCH 09/12] better node graph --- src/ovis_bringup/launch/launch.py | 31 ++++++++++++++++--- .../config/default_bridge.yaml | 8 ----- .../config/ovis_simulated_config.yaml | 6 ++-- src/ovis_servo/launch/servo.launch.py | 5 ++- src/ovis_servo/src/joystick_servo.cpp | 26 +++++++++------- 5 files changed, 47 insertions(+), 29 deletions(-) diff --git a/src/ovis_bringup/launch/launch.py b/src/ovis_bringup/launch/launch.py index afbab3e..7d277bf 100644 --- a/src/ovis_bringup/launch/launch.py +++ b/src/ovis_bringup/launch/launch.py @@ -4,6 +4,8 @@ from launch.substitutions import Command from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.actions import IncludeLaunchDescription +from launch.actions import ExecuteProcess, IncludeLaunchDescription, RegisterEventHandler +from launch.event_handlers import OnProcessExit from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue @@ -32,6 +34,7 @@ def generate_launch_description(): robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', + namespace='/ovis', name='robot_state_publisher', output='both', parameters=[ @@ -59,6 +62,7 @@ def generate_launch_description(): # Fake joint driver fake_joint_driver = Node( package='controller_manager', + namespace='/ovis', executable='ros2_control_node', parameters=[ {'robot_description': robot_desc}, @@ -67,10 +71,16 @@ def generate_launch_description(): ], ) - spawn_controllers_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_ovis_moveit, 'launch', 'spawn_controllers.launch.py') - ) + load_joint_state_broadcaster = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'joint_state_broadcaster', '-c', '/ovis/controller_manager'], + output='screen' + ) + + load_arm_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'arm_controller', '-c', '/ovis/controller_manager'], + output='screen' ) servo = IncludeLaunchDescription( @@ -80,11 +90,22 @@ def generate_launch_description(): ) return LaunchDescription([ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_state_publisher, + on_exit=[load_joint_state_broadcaster], + ) + ), + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_joint_state_broadcaster, + on_exit=[load_arm_controller], + ) + ), virtual_joints_launch, robot_state_publisher, move_group_launch, rviz_launch, fake_joint_driver, - spawn_controllers_launch, servo, ]) diff --git a/src/ovis_description/config/default_bridge.yaml b/src/ovis_description/config/default_bridge.yaml index 71a25b0..31c1f70 100644 --- a/src/ovis_description/config/default_bridge.yaml +++ b/src/ovis_description/config/default_bridge.yaml @@ -4,11 +4,3 @@ ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "gz.msgs.Clock" direction: GZ_TO_ROS - -# gz topic published by JointState plugin -- ros_topic_name: "joint_states" - gz_topic_name: "joint_states" - ros_type_name: "sensor_msgs/msg/JointState" - gz_type_name: "gz.msgs.Model" - direction: GZ_TO_ROS -# remove gz topic published by JointState plugin diff --git a/src/ovis_servo/config/ovis_simulated_config.yaml b/src/ovis_servo/config/ovis_simulated_config.yaml index bfaa63a..3963c6f 100644 --- a/src/ovis_servo/config/ovis_simulated_config.yaml +++ b/src/ovis_servo/config/ovis_simulated_config.yaml @@ -58,10 +58,10 @@ joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: /ovis/servo_node/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: /ovis/servo_node/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /ovis/joint_states -status_topic: ~/status # Publish status to this topic +status_topic: /ovis/status # Publish status to this topic command_out_topic: /ovis/arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body diff --git a/src/ovis_servo/launch/servo.launch.py b/src/ovis_servo/launch/servo.launch.py index 5eb206c..005167c 100644 --- a/src/ovis_servo/launch/servo.launch.py +++ b/src/ovis_servo/launch/servo.launch.py @@ -53,18 +53,21 @@ def generate_launch_description(): # Launch as much as possible in components container = ComposableNodeContainer( name="moveit_servo_demo_container", - namespace="ovis", + namespace="/ovis", package="rclcpp_components", executable="component_container_mt", composable_node_descriptions=[ ComposableNode( package="ovis_servo", + namespace="/ovis", plugin="ovis_servo::JoyToServoPub", name="controller_to_servo_node", parameters=[{'use_sim_time': True}], + remappings=[("/planning_scene", "/ovis/planning_scene")], ), ComposableNode( package="joy", + namespace="/ovis", plugin="joy::Joy", name="joy_node", parameters=[{'use_sim_time': True}], diff --git a/src/ovis_servo/src/joystick_servo.cpp b/src/ovis_servo/src/joystick_servo.cpp index 95d29ed..867dbe9 100644 --- a/src/ovis_servo/src/joystick_servo.cpp +++ b/src/ovis_servo/src/joystick_servo.cpp @@ -56,9 +56,9 @@ #include // We'll just set up parameters here -const std::string JOY_TOPIC = "/joy"; -const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; -const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; +const std::string JOY_TOPIC = "/ovis/joy"; +const std::string TWIST_TOPIC = "/ovis/servo_node/delta_twist_cmds"; +const std::string JOINT_TOPIC = "/ovis/servo_node/delta_joint_cmds"; const std::string EEF_FRAME_ID = "ovis_end_effector"; const std::string BASE_FRAME_ID = "ovis_link_base"; @@ -112,7 +112,6 @@ bool convertJoyToCmd(const std::vector& axes, const std::vector& but // If any joint jog command is requested, we are only publishing joint commands if (buttons[A] || buttons[B] || buttons[X] || buttons[Y] || axes[D_PAD_X] || axes[D_PAD_Y]) { - // Map the D_PAD to the proximal joints joint->joint_names.push_back("ovis_joint_1"); joint->velocities.push_back(axes[D_PAD_X]); @@ -123,7 +122,7 @@ bool convertJoyToCmd(const std::vector& axes, const std::vector& but joint->joint_names.push_back("ovis_joint_5"); joint->velocities.push_back(buttons[B] - buttons[X]); joint->joint_names.push_back("ovis_joint_3"); - joint->velocities.push_back(buttons[A]- buttons[Y]); + joint->velocities.push_back(buttons[A] - buttons[Y]); return false; } @@ -181,8 +180,10 @@ class JoyToServoPub : public rclcpp::Node servo_start_client_->async_send_request(std::make_shared()); // Initialize gripper action client - gripper_action_client_ = rclcpp_action::create_client( - this, "/robotiq_gripper_controller/gripper_cmd"); + gripper_action_client_ = rclcpp_action::create_client(this, + "/robotiq_gripper_" + "controller/" + "gripper_cmd"); gripper_action_client_->wait_for_action_server(std::chrono::seconds(1)); @@ -246,11 +247,11 @@ class JoyToServoPub : public rclcpp::Node // Check for gripper commands if (msg->buttons[LEFT_STICK_CLICK]) { - sendGripperCommand(1.0, 1.0); // Open gripper + sendGripperCommand(1.0, 1.0); // Open gripper } else if (msg->buttons[RIGHT_STICK_CLICK]) { - sendGripperCommand(0.0, 1.0); // Close gripper + sendGripperCommand(0.0, 1.0); // Close gripper } // Convert the joystick message to Twist or JointJog and publish @@ -277,9 +278,10 @@ class JoyToServoPub : public rclcpp::Node goal_msg.command.max_effort = max_effort; auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = [](const rclcpp_action::ClientGoalHandle::WrappedResult & result) { - // Handle result here - }; + send_goal_options.result_callback = + [](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + // Handle result here + }; gripper_action_client_->async_send_goal(goal_msg, send_goal_options); } From 1d246397cd6479eddf7e1019550d5da0bf303bca Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Tue, 27 Aug 2024 00:06:24 -0400 Subject: [PATCH 10/12] fix rviz2 trajectory issue --- src/ovis_bringup/launch/sim.launch.py | 6 +- src/ovis_moveit/config/moveit.rviz | 49 +------------ .../config/moveit_controllers.yaml | 2 + .../config/ovis_simulated_config.yaml | 71 ------------------- src/ovis_moveit/launch/move_group.launch.py | 3 +- src/ovis_moveit/launch/moveit_rviz.launch.py | 7 +- src/ovis_moveit/package.xml | 1 - .../config/ovis_simulated_config.yaml | 6 +- 8 files changed, 16 insertions(+), 129 deletions(-) delete mode 100644 src/ovis_moveit/config/ovis_simulated_config.yaml diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 8c448f2..3866818 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -75,7 +75,7 @@ def generate_launch_description(): # Spawn ovis (robot arm) ovis_spawner = Node( package='ros_gz_sim', - namespace=namespace, + # namespace=namespace, executable='create', arguments=['-name', 'ovis', '-topic', '/ovis/robot_description', @@ -134,9 +134,9 @@ def generate_launch_description(): robot_state_publisher, set_use_sim_time, move_group_launch, - # rviz_launch, + rviz_launch, gz_sim, bridge, ovis_spawner, - servo, + # servo, ]) diff --git a/src/ovis_moveit/config/moveit.rviz b/src/ovis_moveit/config/moveit.rviz index 3d4f8d7..0d5d2c7 100644 --- a/src/ovis_moveit/config/moveit.rviz +++ b/src/ovis_moveit/config/moveit.rviz @@ -66,53 +66,6 @@ Visualization Manager: Expand Link Details: false Expand Tree: false Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - ovis_end_effector: - Alpha: 1 - Show Axes: false - Show Trail: false - ovis_link_1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ovis_link_base: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false Loop Animation: true Robot Alpha: 0.5 Robot Color: 150; 50; 150 @@ -182,7 +135,7 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 5.233532905578613 + Distance: 12.587531089782715 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 diff --git a/src/ovis_moveit/config/moveit_controllers.yaml b/src/ovis_moveit/config/moveit_controllers.yaml index 793092e..509d858 100644 --- a/src/ovis_moveit/config/moveit_controllers.yaml +++ b/src/ovis_moveit/config/moveit_controllers.yaml @@ -17,3 +17,5 @@ moveit_simple_controller_manager: - ovis_joint_4 - ovis_joint_5 - ovis_joint_6 + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/src/ovis_moveit/config/ovis_simulated_config.yaml b/src/ovis_moveit/config/ovis_simulated_config.yaml deleted file mode 100644 index 28e21f4..0000000 --- a/src/ovis_moveit/config/ovis_simulated_config.yaml +++ /dev/null @@ -1,71 +0,0 @@ -############################################### -# Modify all parameters related to servoing here -############################################### -use_gazebo: false # Whether the robot is started in a Gazebo simulation environment - -## Properties of incoming commands -command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s -scale: - # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. - # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. - joint: 0.5 - -# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) -# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] - -## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] -low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) - -# What type of topic does your robot driver expect? -# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory -command_out_type: trajectory_msgs/JointTrajectory - -# What to publish? Can save some bandwidth as most robots only require positions or velocities -publish_joint_positions: true -publish_joint_velocities: true -publish_joint_accelerations: false - -## Plugins for smoothing outgoing commands -smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" - -# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, -# which other nodes can use as a source for information about the planning environment. -# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), -# then is_primary_planning_scene_monitor needs to be set to false. -is_primary_planning_scene_monitor: true - -## MoveIt properties -move_group_name: arm # Often 'manipulator' or 'arm' -planning_frame: ovis_link_base # The MoveIt planning frame. Often 'base_link' or 'world' - -## Other frames -ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: ovis_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector - -## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command -# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. -# Important because ROS may drop some messages and we need the robot to halt reliably. -num_outgoing_halt_msgs_to_publish: 4 - -## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) -hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this -joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. -leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) - -## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands -joint_topic: /joint_states -status_topic: ~/status # Publish status to this topic -command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here - -## Collision checking for the entire robot body -check_collisions: true # Check collisions? -collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. -self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] -scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/src/ovis_moveit/launch/move_group.launch.py b/src/ovis_moveit/launch/move_group.launch.py index 8485322..ee09fa7 100644 --- a/src/ovis_moveit/launch/move_group.launch.py +++ b/src/ovis_moveit/launch/move_group.launch.py @@ -30,9 +30,10 @@ def generate_launch_description(): package="moveit_ros_move_group", executable="move_group", output="screen", - namespace="ovis", + # namespace="ovis", parameters=move_group_params, # Set the display variable, in case OpenGL code is used internally additional_env={"DISPLAY": ":0"}, + remappings=[('/joint_states', '/ovis/joint_states')], ) return LaunchDescription([move_group]) \ No newline at end of file diff --git a/src/ovis_moveit/launch/moveit_rviz.launch.py b/src/ovis_moveit/launch/moveit_rviz.launch.py index 8e3162c..92f40dd 100644 --- a/src/ovis_moveit/launch/moveit_rviz.launch.py +++ b/src/ovis_moveit/launch/moveit_rviz.launch.py @@ -1,4 +1,5 @@ from ast import arg +import re from moveit_configs_utils import MoveItConfigsBuilder from moveit_configs_utils.launches import generate_moveit_rviz_launch from launch import LaunchDescription @@ -16,11 +17,13 @@ def generate_launch_description(): moveit_rviz = Node( package="rviz2", executable="rviz2", - namespace="ovis", - output="log", + #namespace="ovis", + output="both", respawn=False, arguments=["-d", str(moveit_config.package_path / "config/moveit.rviz")], parameters=rviz_parameters, + remappings=[('/robot_description', '/ovis/robot_description'), + ], ) return LaunchDescription([moveit_rviz]) \ No newline at end of file diff --git a/src/ovis_moveit/package.xml b/src/ovis_moveit/package.xml index 8595f41..9e1136e 100644 --- a/src/ovis_moveit/package.xml +++ b/src/ovis_moveit/package.xml @@ -34,7 +34,6 @@ moveit_configs_utils moveit_ros_move_group moveit_ros_visualization - moveit_ros_warehouse moveit_setup_assistant ovis_description robot_state_publisher diff --git a/src/ovis_servo/config/ovis_simulated_config.yaml b/src/ovis_servo/config/ovis_simulated_config.yaml index 3963c6f..bfaa63a 100644 --- a/src/ovis_servo/config/ovis_simulated_config.yaml +++ b/src/ovis_servo/config/ovis_simulated_config.yaml @@ -58,10 +58,10 @@ joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names -cartesian_command_in_topic: /ovis/servo_node/delta_twist_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: /ovis/servo_node/delta_joint_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /ovis/joint_states -status_topic: /ovis/status # Publish status to this topic +status_topic: ~/status # Publish status to this topic command_out_topic: /ovis/arm_controller/joint_trajectory # Publish outgoing commands here ## Collision checking for the entire robot body From a372d5ad885e652aae307cb3bffdda5fe4339734 Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Tue, 27 Aug 2024 18:35:32 -0400 Subject: [PATCH 11/12] remove namespace for joint state --- src/ovis_description/urdf/gazebo.urdf.xacro | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/ovis_description/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/gazebo.urdf.xacro index 3e44858..62a1f45 100644 --- a/src/ovis_description/urdf/gazebo.urdf.xacro +++ b/src/ovis_description/urdf/gazebo.urdf.xacro @@ -21,9 +21,6 @@ filename="gz-sim-joint-state-publisher-system" name="gz::sim::systems::JointStatePublisher"> joint_states - - /ovis - Date: Wed, 28 Aug 2024 21:59:48 -0400 Subject: [PATCH 12/12] trajectory planner support with rove --- src/ovis_bringup/launch/launch.py | 4 +- src/ovis_bringup/launch/sim.launch.py | 11 +- .../config/default_bridge.yaml | 7 + src/ovis_description/urdf/gazebo.urdf.xacro | 14 -- .../ovis2/urdf/ovis_standalone.urdf.xacro | 7 +- .../urdf/ovis_standalone.urdf.xacro | 25 ++-- src/ovis_description/worlds/base_world.world | 128 +++++++++-------- src/ovis_moveit/config/moveit.rviz | 4 +- src/ovis_moveit/config/ovis.srdf | 133 +++++++++++------- src/ovis_servo/config/ovis_gazebo_config.yaml | 22 +-- .../config/ovis_simulated_config.yaml | 2 +- .../ovis_simulated_config_pose_tracking.yaml | 20 +-- src/ovis_servo/launch/servo_gazebo.launch.py | 2 +- 13 files changed, 215 insertions(+), 164 deletions(-) diff --git a/src/ovis_bringup/launch/launch.py b/src/ovis_bringup/launch/launch.py index 7d277bf..7a693ac 100644 --- a/src/ovis_bringup/launch/launch.py +++ b/src/ovis_bringup/launch/launch.py @@ -22,6 +22,8 @@ def generate_launch_description(): # Get the launch directory pkg_ovis_moveit = get_package_share_directory('ovis_moveit') + namespace = '/ovis' + # Include launch files based on the configuration virtual_joints_launch = IncludeLaunchDescription( @@ -34,7 +36,7 @@ def generate_launch_description(): robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', - namespace='/ovis', + namespace=namespace, name='robot_state_publisher', output='both', parameters=[ diff --git a/src/ovis_bringup/launch/sim.launch.py b/src/ovis_bringup/launch/sim.launch.py index 3866818..53fe675 100644 --- a/src/ovis_bringup/launch/sim.launch.py +++ b/src/ovis_bringup/launch/sim.launch.py @@ -117,6 +117,14 @@ def generate_launch_description(): output='screen' ) + static_transform_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + arguments=['0', '0', '0', '0', '0', '0', 'world', 'ovis_base_link'], + output='screen' + ) + return LaunchDescription([ RegisterEventHandler( @@ -131,6 +139,7 @@ def generate_launch_description(): on_exit=[load_arm_controller], ) ), + # static_transform_publisher, robot_state_publisher, set_use_sim_time, move_group_launch, @@ -138,5 +147,5 @@ def generate_launch_description(): gz_sim, bridge, ovis_spawner, - # servo, + servo, ]) diff --git a/src/ovis_description/config/default_bridge.yaml b/src/ovis_description/config/default_bridge.yaml index 31c1f70..5fc953a 100644 --- a/src/ovis_description/config/default_bridge.yaml +++ b/src/ovis_description/config/default_bridge.yaml @@ -4,3 +4,10 @@ ros_type_name: "rosgraph_msgs/msg/Clock" gz_type_name: "gz.msgs.Clock" direction: GZ_TO_ROS + +# gz topic published by JointState plugin +- ros_topic_name: "joint_states" + gz_topic_name: "joint_states" + ros_type_name: "sensor_msgs/msg/JointState" + gz_type_name: "gz.msgs.Model" + direction: GZ_TO_ROS diff --git a/src/ovis_description/urdf/gazebo.urdf.xacro b/src/ovis_description/urdf/gazebo.urdf.xacro index 62a1f45..3775390 100644 --- a/src/ovis_description/urdf/gazebo.urdf.xacro +++ b/src/ovis_description/urdf/gazebo.urdf.xacro @@ -14,18 +14,4 @@ - - - - - joint_states - - - ogre2 - - \ No newline at end of file diff --git a/src/ovis_description/urdf/ovis2/urdf/ovis_standalone.urdf.xacro b/src/ovis_description/urdf/ovis2/urdf/ovis_standalone.urdf.xacro index fc52caa..4d88cdc 100644 --- a/src/ovis_description/urdf/ovis2/urdf/ovis_standalone.urdf.xacro +++ b/src/ovis_description/urdf/ovis2/urdf/ovis_standalone.urdf.xacro @@ -3,15 +3,14 @@ - - - - + + + diff --git a/src/ovis_description/urdf/ovis_standalone.urdf.xacro b/src/ovis_description/urdf/ovis_standalone.urdf.xacro index 9ecb600..8e49e91 100644 --- a/src/ovis_description/urdf/ovis_standalone.urdf.xacro +++ b/src/ovis_description/urdf/ovis_standalone.urdf.xacro @@ -3,18 +3,20 @@ - - + - + + - - + + - + @@ -22,16 +24,13 @@ - + - - - + + + - - - \ No newline at end of file diff --git a/src/ovis_description/worlds/base_world.world b/src/ovis_description/worlds/base_world.world index 2c8dda5..9dc4d65 100644 --- a/src/ovis_description/worlds/base_world.world +++ b/src/ovis_description/worlds/base_world.world @@ -1,64 +1,74 @@ - + - - - - 2 18 15 0 0.75 -1.7 - - - + + + + 2 18 15 0 0.75 -1.7 + + - - 0.001 - 1 - 1000 - - - - - + + 0.001 + 1 + 1000 + - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - - - - true - - - - - 0 0 1 - 100 100 - - - - - - - 0 0 1 - 100 100 - - - - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - 0.8 0.8 0.8 1 - - - - - + + + + + + + + joint_states + + + ogre2 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + \ No newline at end of file diff --git a/src/ovis_moveit/config/moveit.rviz b/src/ovis_moveit/config/moveit.rviz index 0d5d2c7..45e1465 100644 --- a/src/ovis_moveit/config/moveit.rviz +++ b/src/ovis_moveit/config/moveit.rviz @@ -120,7 +120,7 @@ Visualization Manager: Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: base_link + Fixed Frame: ovis_base_link Frame Rate: 30 Name: root Tools: @@ -151,7 +151,7 @@ Visualization Manager: Name: Current View Near Clip Distance: 0.009999999776482582 Pitch: 0.5949999690055847 - Target Frame: base_link + Target Frame: ovis_base_link Value: Orbit (rviz_default_plugins) Yaw: 4.385163307189941 Saved: ~ diff --git a/src/ovis_moveit/config/ovis.srdf b/src/ovis_moveit/config/ovis.srdf index 5c6e293..cfd8566 100644 --- a/src/ovis_moveit/config/ovis.srdf +++ b/src/ovis_moveit/config/ovis.srdf @@ -1,58 +1,97 @@ - - - - - - + + + + + - - - - - - - - - + + + + + + + + + - + - - - - - - + + + + + + - - + + - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + - + \ No newline at end of file diff --git a/src/ovis_servo/config/ovis_gazebo_config.yaml b/src/ovis_servo/config/ovis_gazebo_config.yaml index 6f3388b..41698f4 100644 --- a/src/ovis_servo/config/ovis_gazebo_config.yaml +++ b/src/ovis_servo/config/ovis_gazebo_config.yaml @@ -7,8 +7,8 @@ use_gazebo: True # Whether the robot is started in a Gazebo simulation environme command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.4 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.6 @@ -16,8 +16,8 @@ scale: # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] ## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] -low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) +publish_period: 0.034 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory @@ -38,27 +38,27 @@ smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" is_primary_planning_scene_monitor: true ## MoveIt properties -move_group_name: ovis_manipulator # Often 'manipulator' or 'arm' -planning_frame: ovis_link_base # The MoveIt planning frame. Often 'base_link' or 'world' +move_group_name: ovis_manipulator # Often 'manipulator' or 'arm' +planning_frame: ovis_base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: ovis_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: ovis_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits -lower_singularity_threshold: 50.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 50.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 75.0 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /ovis/joint_states status_topic: ~/status # Publish status to this topic diff --git a/src/ovis_servo/config/ovis_simulated_config.yaml b/src/ovis_servo/config/ovis_simulated_config.yaml index bfaa63a..df84225 100644 --- a/src/ovis_servo/config/ovis_simulated_config.yaml +++ b/src/ovis_servo/config/ovis_simulated_config.yaml @@ -43,7 +43,7 @@ planning_frame: ovis_link_base # The MoveIt planning frame. Often 'base_link' or ## Other frames ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: ovis_link_base # commands must be given in the frame of a robot link. Usually either the base or end effector +robot_link_command_frame: ovis_base_link # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command diff --git a/src/ovis_servo/config/ovis_simulated_config_pose_tracking.yaml b/src/ovis_servo/config/ovis_simulated_config_pose_tracking.yaml index c990a87..7a12110 100644 --- a/src/ovis_servo/config/ovis_simulated_config_pose_tracking.yaml +++ b/src/ovis_servo/config/ovis_simulated_config_pose_tracking.yaml @@ -7,8 +7,8 @@ use_gazebo: false # Whether the robot is started in a Gazebo simulation environm command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s scale: # Scale parameters are only used if command_in_type=="unitless" - linear: 0.6 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + linear: 0.6 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 0.3 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. joint: 0.01 @@ -16,7 +16,7 @@ scale: # override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0] ## Properties of outgoing commands -publish_period: 0.034 # 1/Nominal publish rate [seconds] +publish_period: 0.034 # 1/Nominal publish rate [seconds] # What type of topic does your robot driver expect? # Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory @@ -31,27 +31,27 @@ publish_joint_accelerations: false smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" ## MoveIt properties -move_group_name: ovis_manipulator # Often 'manipulator' or 'arm' -planning_frame: ovis_link_base # The MoveIt planning frame. Often 'base_link' or 'world' +move_group_name: ovis_manipulator # Often 'manipulator' or 'arm' +planning_frame: ovis_base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: ovis_link_6 # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: ovis_link_6 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: ovis_link_6 # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits -lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) +lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) ## Topic names -cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands +cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: /joint_states status_topic: ~/status # Publish status to this topic diff --git a/src/ovis_servo/launch/servo_gazebo.launch.py b/src/ovis_servo/launch/servo_gazebo.launch.py index 96cd2c7..476d19f 100644 --- a/src/ovis_servo/launch/servo_gazebo.launch.py +++ b/src/ovis_servo/launch/servo_gazebo.launch.py @@ -143,7 +143,7 @@ def generate_launch_description(): package="tf2_ros", plugin="tf2_ros::StaticTransformBroadcasterNode", name="static_tf2_broadcaster", - parameters=[{"child_frame_id": "/ovis_link_base", "frame_id": "/base_link", "use_sim_time": True}], + parameters=[{"child_frame_id": "/ovis_link_base", "frame_id": "/ovis_base_link", "use_sim_time": True}], ), ComposableNode( package="ovis_servo",