Use of Fakesystem

[antonio1matos]

Hello,
I was trying to launch the robot iiwa14 in Rviz with MoveItConfigsBuilder. I made a code similar to the launch file demo_launch that is present in moveit2 humble tutorials (ros2 launch moveit2_tutorials demo.launch.py) that is used to launch the robot panda in the rviz -> https://moveit.picknik.ai/humble/doc/tutorials/your_first_project/your_first_project.html

I tried to adapt that code to the package lbr_fri_ros2_stack using the robot iiwa14:
this is my code:

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
from lbr_description import LBRDescriptionMixin, RVizMixin
from lbr_bringup import LBRMoveGroupMixin

def generate_launch_description():

declared_arguments = []
declared_arguments.append(
    DeclareLaunchArgument(
        "rviz_config",
        default_value="miariiwa1.rviz",
        description="RViz configuration file",
    )
)

return LaunchDescription(
    declared_arguments + [OpaqueFunction(function=launch_setup)]
)

def launch_setup(context, *args, **kwargs):

moveit_config = (
    MoveItConfigsBuilder("iiwa14")
    .robot_description(os.path.join(
            get_package_share_directory("lbr_description"),
            "urdf/iiwa14/iiwa14.urdf.xacro",
        )
    )
    .trajectory_execution(file_path="config/moveit_controllers.yaml")
    .planning_scene_monitor(
        publish_robot_description=True, publish_robot_description_semantic=True
    )
    .planning_pipelines(
        pipelines=["ompl", "pilz_industrial_motion_planner"]
    )
    .robot_description_kinematics(file_path="config/kinematics.yaml")
    .to_moveit_configs()   
)

# Start the actual move_group node/action server
run_move_group_node = Node(
    package="moveit_ros_move_group",
    executable="move_group",
    output="screen",
    parameters=[moveit_config.to_dict()],
)

rviz_base = LaunchConfiguration("rviz_config")
rviz_config = PathJoinSubstitution(
    #[FindPackageShare("lbr_description"), "config", rviz_base]
    [FindPackageShare("miar_robot"), "launch", rviz_base]
    
)

# RViz
rviz_node = Node(
    package="rviz2",
    executable="rviz2",
    name="rviz2",
    output="log",
    arguments=["-d", rviz_config],
    parameters=[
        moveit_config.robot_description,
        moveit_config.robot_description_semantic,
        moveit_config.robot_description_kinematics,
        moveit_config.planning_pipelines,
        moveit_config.joint_limits,
    ],
)

# Static TF
static_tf = Node(
    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"],
)

# Publish TF
robot_state_publisher = Node(
    package="robot_state_publisher",
    executable="robot_state_publisher",
    name="robot_state_publisher",
    output="both",
    parameters=[moveit_config.robot_description],
)

# ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
    get_package_share_directory("lbr_ros2_control"),
    "config",
    "lbr_controllers.yaml",
)
ros2_control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[moveit_config.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=["joint_trajectory_controller", "-c", "/controller_manager"],
)

nodes_to_start = [
    rviz_node,
    static_tf,
    robot_state_publisher,
    run_move_group_node,
    ros2_control_node,
    joint_state_broadcaster_spawner,
    arm_controller_spawner,
]

return nodes_to_start

---

In this code in the line: default_value="miariiwa1.rviz", I call the rviz file miariiwa1.rviz that itś not present in the package br_fri_ros2_stack. That code was created in a other package that i create called "miar_robot" however the miariiwa1.rviz code is attached below.
miariiwa1.zip

The code works. The robot appears in the rviz and i can use the command Plan for the robot plan a trajectory (using the planner OMPL for example) however the command Execute or Plan & Execute doesnt work. I think itś because moveit itś not reading the controller manager cus when i run the launch file appears in the terminal: [spawner-6] [INFO] [1712674899.497416360] [spawner_joint_state_broadcaster]: Waiting for ‘/controller_manager’ node to exist . Somehow i think moveit itś not reading the file: # ros2_control using FakeSystem as hardware
ros2_controllers_path = os.path.join(
get_package_share_directory("lbr_ros2_control"),
"config",
"lbr_controllers.yaml",
)

[mhubii]

okay, so I'll try to understand your code in more detail in a bit. Just a brief question, the lbr_bringup uses the config builder, see e.g.

lbr_fri_ros2_stack/lbr_bringup/launch/real.launch.py

Line 92 in 036b23b

moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(

Is there anything that the lbr_brinup doesn't provide that you need? Just so I understand where you are coming from better.

Essentially you are trying to use different RViz configs, is that correct?

For Moveit to function with RViz, you'll have to pass specific paramters to RViz, see

lbr_fri_ros2_stack/lbr_bringup/launch/real.launch.py

Line 114 in 036b23b

parameters=LBRMoveGroupMixin.params_rviz(

[antonio1matos]

In that case i will have to do a code similar to the one in lbr_fri_ros2_stack/lbr_bringup/launch/real.launch.py ?

[mhubii]

are you running in simulation or the real robot?

[antonio1matos]

Iḿ running in simulation

[mhubii]

okay in this case you could have a look at https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_bringup/launch/sim.launch.py

In this launch file, Gazebo etc is launched. You should be able to modify the RViz config to your needs

lbr_fri_ros2_stack/lbr_bringup/launch/sim.launch.py

Line 89 in 036b23b

rviz_config_pkg=f"{model}_moveit_config",

But I can see how this would not work out of the box with custom moveit configs since these configs are somewhat hard coded

lbr_fri_ros2_stack/lbr_bringup/lbr_bringup/launch_mixin.py

Line 56 in 036b23b

def moveit_configs_builder(

Do you happen to have a different URDF etc?

[antonio1matos]

no

[mhubii]

Okay so you could do:

• Launch using ros2 launch lbr_bringup bringup.launch.py model:=iiwa14 moveit:=true
• Configure RViz to your needs
• Save configs: In RViz File -> Save Config As (save to your package)
• Copy paste the launch file and update the origin RViz config file to your saved file, here:
  

  lbr_fri_ros2_stack/lbr_bringup/launch/sim.launch.py

  Line 89 in 036b23b

  rviz_config_pkg=f"{model}_moveit_config",

Or do you wish to avoid Gazebo?

[antonio1matos]

Do you have any code using the iiwa1a from the package br_fri_ros2_stack where the robot from to a desired position (goal pose)? I need to do a pick and place projec where i was planning to use the move_group to control the robot. My idea was to use a launch file to appear the robot in the rviz and then i would try to use a node to execute the movements of the robot. If you could provide me with something to make my project easier, I would greatly appreciate it. mhubii ***@***.***> escreveu (terça, 9/04/2024 à(s) 18:19):

…

Okay so you could do: - Launch using ros2 launch lbr_bringup bringup.launch.py model:=iiwa14 moveit:=true - Configure RViz to your needs - Save configs: In RViz File -> Save Config As (save to your package) - Copy paste the launch file and update the origin RViz config file to your saved file, here: https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/036b23b2f91af70e1ca490e0c81660b6b17ae114/lbr_bringup/launch/sim.launch.py#L89 — Reply to this email directly, view it on GitHub <#166 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/BHWRKN2FOMMYFI7D7V27XZTY4QPLLAVCNFSM6AAAAABF62M222VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDANBVG4ZDQNRTGI> . You are receiving this because you authored the thread.Message ID: ***@***.***>

[antonio1matos]

Yes would prefer to avoid gazebbo, amd only use rviz

[mhubii]

yes, you can e.g. checkout

#133 (comment)
and
#152 (comment)

Closing this issue for now, as it seems somewhat unrelated to this repo, but more general. You might need to simulated physics if you are planning to do grasping, RViz might not suffice.

[antonio1matos]

Hi i was trying to use the code from ros2 launch lbr_bringup sim.launch.py. But i changed the code to the model be iiwa14, instead of iiwa7. Here it is: from typing import List

from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.substitutions import (
AndSubstitution,
LaunchConfiguration,
NotSubstitution,
PathJoinSubstitution,
)

from lbr_bringup import LBRMoveGroupMixin
from lbr_description import GazeboMixin, LBRDescriptionMixin, RVizMixin
from lbr_ros2_control import LBRROS2ControlMixin
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition, UnlessCondition
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder
from lbr_description import LBRDescriptionMixin, RVizMixin
from lbr_bringup import LBRMoveGroupMixin

def launch_setup(context: LaunchContext) -> List[LaunchDescriptionEntity]:
ld = LaunchDescription()

robot_description = LBRDescriptionMixin.param_robot_description(sim=True)
ld.add_action(GazeboMixin.include_gazebo())  # Gazebo has its own controller manager
spawn_entity = GazeboMixin.node_spawn_entity()
ld.add_action(spawn_entity)
joint_state_broadcaster = LBRROS2ControlMixin.node_controller_spawner(
    controller="joint_state_broadcaster"
)
ld.add_action(joint_state_broadcaster)
robot_state_publisher = LBRROS2ControlMixin.node_robot_state_publisher(
    robot_description=robot_description, use_sim_time=True
)
ld.add_action(
    robot_state_publisher
)  # Do not condition robot state publisher on joint state broadcaster as Gazebo uses robot state publisher to retrieve robot description
ld.add_action(
    LBRROS2ControlMixin.node_controller_spawner(
        controller=LaunchConfiguration("ctrl")
    )
)

# MoveIt 2
ld.add_action(LBRMoveGroupMixin.arg_allow_trajectory_execution())
ld.add_action(LBRMoveGroupMixin.arg_capabilities())
ld.add_action(LBRMoveGroupMixin.arg_disable_capabilities())
ld.add_action(LBRMoveGroupMixin.arg_monitor_dynamics())
ld.add_action(LBRMoveGroupMixin.args_publish_monitored_planning_scene())

# MoveGroup:
# - requires world frame
# - urdf only has robot_name/world
# This transform needs publishing
robot_name = LaunchConfiguration("robot_name").perform(context)
ld.add_action(
    LBRDescriptionMixin.node_static_tf(
        tf=[0, 0, 0, 0, 0, 0],  # keep zero
        parent="world",
        child=PathJoinSubstitution(
            [
                robot_name,
                "world",
            ]  # results in robot_name/world
        ),
        parameters=[{"use_sim_time": True}],
    )
)

ld.add_action(DeclareLaunchArgument(
name="model",
default_value="iiwa14",
description="The robot model to use."
))

ld.add_action(DeclareLaunchArgument(
    name="moveit",
    default_value="true",
    description="Whether to launch MoveIt 2."
))


model = LaunchConfiguration("model").perform(context)
moveit_configs_builder = LBRMoveGroupMixin.moveit_configs_builder(
    robot_name=model,
    package_name=f"{model}_moveit_config",
)
movegroup_params = LBRMoveGroupMixin.params_move_group()

ld.add_action(
    LBRMoveGroupMixin.node_move_group(
        parameters=[
            moveit_configs_builder.to_dict(),
            movegroup_params,
            {"use_sim_time": True},
        ],
        condition=IfCondition(LaunchConfiguration("moveit")),
        namespace=robot_name,
    )
)

# RViz and MoveIt
rviz_moveit = RVizMixin.node_rviz(
    rviz_config_pkg=f"{model}_moveit_config",
    rviz_config="config/moveit.rviz",
    parameters=LBRMoveGroupMixin.params_rviz(
        moveit_configs=moveit_configs_builder.to_moveit_configs()
    )
    + [{"use_sim_time": True}],
    condition=IfCondition(
        AndSubstitution(LaunchConfiguration("moveit"), LaunchConfiguration("rviz"))
    ),
    remappings=[
        ("display_planned_path", robot_name + "/display_planned_path"),
        ("joint_states", robot_name + "/joint_states"),
        ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
        ("planning_scene", robot_name + "/planning_scene"),
        ("planning_scene_world", robot_name + "/planning_scene_world"),
        ("robot_description", robot_name + "/robot_description"),
        ("robot_description_semantic", robot_name + "/robot_description_semantic"),
    ],
)

# RViz no MoveIt
rviz = RVizMixin.node_rviz(
    rviz_config_pkg="lbr_bringup",
    rviz_config="config/config.rviz",
    condition=IfCondition(
        AndSubstitution(
            LaunchConfiguration("rviz"),
            NotSubstitution(LaunchConfiguration("moveit")),
        )
    ),
)

# RViz event handler
rviz_event_handler = RegisterEventHandler(
    OnProcessExit(target_action=spawn_entity, on_exit=[rviz_moveit, rviz])
)
ld.add_action(rviz_event_handler)

return ld.entities

def generate_launch_description() -> LaunchDescription:
ld = LaunchDescription()
ld.add_action(LBRDescriptionMixin.arg_model())
ld.add_action(LBRDescriptionMixin.arg_robot_name())
ld.add_action(
DeclareLaunchArgument(
name="moveit",
default_value="true",
description="Whether to launch MoveIt 2.",
)
)
ld.add_action(
DeclareLaunchArgument(
name="rviz", default_value="true", description="Whether to launch RViz."
)
)

ld.add_action(
    LBRROS2ControlMixin.arg_ctrl()
)  # Gazebo loads controller configuration through lbr_description/gazebo/*.xacro from lbr_ros2_control/config/lbr_controllers.yaml


ld.add_action(OpaqueFunction(function=launch_setup))
return ld

I added this: ld.add_action(DeclareLaunchArgument(
name="model",
default_value="iiwa14",
description="The robot model to use."
))
However when i do: ros2 launch lbr_bringup si.launch.py in the output it reads the model iiwa7 instead of iiwa14, and i dnt know why.Do you know the reason?

[mhubii]

hey, you can just do

ros2 launch lbr_bringup bringup.launch.py model:=iiwa14 moveit:=true

This will launch the simulation with the iiwa14 model and moveit

[antonio1matos]

Yes i know, but i wanted to run it in the code. I just wanted to put in the terminal: ros2 launch lbr_bringup bringup.launch.py (i don't want to write everytime model:=iiwa14 moveit:=true)
However somehow the output is still iiwa7.
Still thank you for your attention

[antonio1matos]

Ok, i already solve the problem. I had to change the default value from iiwa7 to iiwa14 in the file launch_mixin.py in lbr_description

[antonio1matos]

Hello. I noticed that if you wanted to use ros2 launch lbr_bringup bringup.launch.py model:=iiwa14 moveit:=true you cant´t use the ompl planner and you cant even plan adn execute the robot in the rviz. However i changed the robot files that are in iiwa14_moveit_config to be equal to the files that were on the other packages (iiwa7_moveit_config , med7_moveit_config, med14_moveit_config). After making the changes the code ros2 launch lbr_bringup bringup.launch.py model:=iiwa14 moveit:=true worked well. Now i can use the ompl planner with iiwa using that code. You might check that package (iiwa14_moveit_config) with the others becuase that are some differencaces and that's why it wasnt wrkung the planner. Thank you for providign the lbr_fri_ros2_stack

[mhubii]

hm that is quite strange indeed, glad it worked for you in the end

If you don't want to write it in the terminal you can create a new launch file:

from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description() -> LaunchDescription:
    ld = LaunchDescription()

    ld.add_action(
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                PathJoinSubstitution(
                    [
                        FindPackageShare("lbr_bringup"),
                        "launch",
                        "sim.launch.py",
                    ]
                )
            ),
            launch_arguments={
                "model": "iiwa14",
                "moveit": "true",
            }.items(),
        )
    )

    return ld

[antonio1matos]

Yes, thatś a way. Thank you

[antonio1matos]

Hi @mhubii
Im sorry to bother.
I will make a strange question and unrelated to the subsject of this issue. Right now i have a robot arm iiwa doing path planning and i would like to have associated to the end-effector a gripper. I don´t have a custom gripper for the robot arm. I wanted to know if you know if is possible to integrate a gripper of other robot for example the gripper of robot panda that is in the moveit2 tutorials. Obviously this is not an adequated gripper for the robot, but in simulation terms do you know if something like that is possible? Use a gripper of other robot in the iiwa?
Regards

[mhubii]

no worries at all @antonio1matos ! Great question!

Maybe we could try integrating one of these grippers: https://github.com/PickNikRobotics/ros2_robotiq_gripper

Feel free to open a new issue for that!

[antonio1matos]

ok, thank you. I will check that gripper