iiwa in rviz and gazebo
antonio1matos opened this issue · 2 comments
antonio1matos commented
Hello,
I have this code that runs the iiwa in rviz with controllers:
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from ament_index_python.packages import get_package_share_directory
from launch_ros.actions import Node #permite iniciar nodes neste launch file
from launch.launch_description_sources import PythonLaunchDescriptionSource
from typing import Dict, List, Optional, Union
def generate_launch_description():
# Declare arguments
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
'description_package',
default_value='iiwa_description',
description='Description package with robot URDF/xacro files. Usually the argument \
is not set, it enables use of a custom description.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'description_file',
default_value='iiwa.config.xacro',
description='URDF/XACRO description file with the robot.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'prefix',
default_value='""',
description='Prefix of the joint names, useful for multi-robot setup. \
If changed than also joint names in the controllers \
configuration have to be updated. Expected format "<prefix>/"',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'namespace',
default_value='/',
description='Namespace of launched nodes, useful for multi-robot setup. \
If changed than also the namespace in the controllers \
configuration needs to be updated. Expected format "<ns>/".',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'start_rviz',
default_value='true',
description='Start RViz2 automatically with this launch file.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'base_frame_file',
default_value='base_frame.yaml',
description='Configuration file of robot base frame wrt World.',
)
)
declared_arguments.append(
DeclareLaunchArgument(
'use_sim',
default_value='false',
description='Start robot in Gazebo simulation.',
)
)
# Initialize Arguments
description_package = LaunchConfiguration('description_package')
description_file = LaunchConfiguration('description_file')
prefix = LaunchConfiguration('prefix')
start_rviz = LaunchConfiguration('start_rviz')
base_frame_file = LaunchConfiguration('base_frame_file')
namespace = LaunchConfiguration('namespace')
use_sim = LaunchConfiguration('use_sim')
# Get URDF via xacro
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name='xacro')]),
' ',
PathJoinSubstitution(
[FindPackageShare(description_package), 'config', description_file]
),
' ',
'prefix:=',
prefix,
' ',
'base_frame_file:=',
base_frame_file,
' ',
'description_package:=',
description_package,
' ',
'namespace:=',
namespace,
]
)
robot_description = {'robot_description': robot_description_content}
# Get SRDF via xacro
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(description_package), "srdf", "iiwa.srdf.xacro"]
),
" ",
"name:=",
"iiwa",
" ",
"prefix:=",
prefix,
" ",
'description_package:=',
description_package,
]
)
robot_description_semantic = {
'robot_description_semantic': robot_description_semantic_content
}
# Get planning parameters
robot_description_planning_joint_limits = PathJoinSubstitution([
FindPackageShare(description_package), "moveit2", "iiwa_joint_limits.yaml",
]
)
robot_description_planning_cartesian_limits = PathJoinSubstitution([
FindPackageShare(description_package), "moveit2", "iiwa_cartesian_limits.yaml",
]
)
move_group_capabilities = {
"capabilities": """pilz_industrial_motion_planner/MoveGroupSequenceAction \
pilz_industrial_motion_planner/MoveGroupSequenceService"""
}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(description_package), "moveit2", "kinematics.yaml"]
)
planning_pipelines_config = PathJoinSubstitution([
FindPackageShare(description_package), "moveit2", "planning_pipelines_config.yaml",
]
)
ompl_planning_config = PathJoinSubstitution([
FindPackageShare(description_package), "moveit2", "ompl_planning.yaml",
]
)
moveit_controllers = PathJoinSubstitution(
[FindPackageShare(description_package),
"moveit2", "iiwa_moveit_controller_config.yaml"]
)
trajectory_execution = {
"moveit_manage_controllers": True,
"trajectory_execution.allowed_execution_duration_scaling": 1.2,
"trajectory_execution.allowed_goal_duration_margin": 0.5,
"trajectory_execution.allowed_start_tolerance": 0.01,
}
planning_scene_monitor_parameters = {
"publish_planning_scene": True,
"publish_geometry_updates": True,
"publish_state_updates": True,
"publish_transforms_updates": True,
}
move_group_node = Node(
package="moveit_ros_move_group",
executable="move_group",
namespace=namespace,
output="screen",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
robot_description_planning_cartesian_limits,
robot_description_planning_joint_limits,
planning_pipelines_config,
ompl_planning_config,
trajectory_execution,
moveit_controllers,
planning_scene_monitor_parameters,
move_group_capabilities,
{"use_sim_time": use_sim},
],
)
#RViz
rviz_config_file = (
get_package_share_directory("miar_robot") + "/launch/miar1.rviz"
)
rviz_node = Node(
package='rviz2',
executable='rviz2',
name='rviz2',
output='log',
arguments=['-d', rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
robot_description_planning_cartesian_limits,
robot_description_planning_joint_limits,
robot_description_kinematics,
planning_pipelines_config,
ompl_planning_config,
],
condition=IfCondition(start_rviz),
)
# Static TF
static_tf = Node( #Um ros node que publica a transformação estática entre os sistemas de coordenas world e panda_link0
package="tf2_ros",
executable="static_transform_publisher",
name="static_transform_publisher",
output="log",
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "link_0"], #coordenadas de translação (os 3 primeiros 0); coordenadas de rotação os 3 segundos zeros;world é o nome de sistemas de coordenadas global ou de referencia; (panda_link0 no caso do robot panda) link_0 é o sistema de coordenadas que está sendo transformado em relação ao sistema de coordenadas de referencia "world"
)
# Publish TF
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="both",
parameters=[robot_description],
)
# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("iiwa_description"),
"config",
"iiwa_controllers.yaml",
)
ros2_control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, ros2_controllers_path],
output="both",
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=[
"joint_state_broadcaster",
"--controller-manager-timeout",
"300",
"--controller-manager",
"/controller_manager",
],
)
arm_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["iiwa_arm_controller", "-c", "/controller_manager"],
)
nodes = [
move_group_node,
rviz_node,
static_tf,
robot_state_publisher,
ros2_control_node,
joint_state_broadcaster_spawner,
arm_controller_spawner,
]
return LaunchDescription(declared_arguments + nodes)The code works fine. I'm able to launch the robot in the rviz and generate trajectories. DO you know what i have to add to this code for the robot be able to be seen also working in gazebo??
tpoignonec commented
Hi!
Sorry for the late reply...
We haven't used Gazebo much with this stack, but in theory you can run the controller_manager in a Gazebo plugin.
Did you take a look at gazebo_ros2_control (for Gazebo classic) or gz_ros2_control (for Gazebo ignition, now the "normal" Gazebo version)?
antonio1matos commented
Hello. Thank you for the reply.
Eventually i was able to launch the gripper with the gazebo simulation.
I used, as you said, the gazebo_ros2_control as the hardware plugin for the ros2_controllers.
Thank you.