Move it - Githubissues

josefinemonnet commented 10 months ago

Could you maybe provide a little demo script using move it to control the robot? Would be really helpful for me.

Thank you in advance.

mhubii commented 10 months ago

Hi @josefinemonnet , yes will definitely work on that. A little busy this week though.

Nicolai-98 commented 2 months ago

I am trying to find the move group of the arm right now using a little script but I am having some trouble. The Plan is to later use move_it to control the arm more precisely. For example making sure movements are deterministic even on 7DOF using additional parameters like status and turn (KUKA style).

First I start the launch script hoping it starts the move group. ros2 launch lbr_bringup bringup.launch.py sim:=true moveit:=true
- Note I am seeing some output that could be related to the problem:
  - [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?
  - [rviz2-9] [INFO] [1721117634.530722284] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to call service get_planning_scene, have you launched move_group or called psm.providePlanningSceneService()?

Then I launch my c++ node:


#include <memory> 
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

int main(int argc, char *argv[]) { // Initialize ROS and create the Node rclcpp::init(argc, argv); auto const node = std::make_shared( "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); auto const logger = rclcpp::get_logger("hello_moveit"); using moveit::planning_interface::MoveGroupInterface; auto move_group_interface = MoveGroupInterface(node, "arm"); ...

Output:
```bash
[ERROR] [1721118585.081668920] [hello_moveit]: Could not find parameter robot_description and did not receive robot_description via std_msgs::msg::String subscription within 10.000000 seconds.
Error:   Error document empty.
         at line 100 in ./urdf_parser/src/model.cpp
Failed to parse robot description using: urdf_xml_parser/URDFXMLParser
[INFO] [1721118595.089338332] [moveit_rdf_loader.rdf_loader]: Unable to parse URDF
[FATAL] [1721118595.089669561] [move_group_interface]: Unable to construct robot model. Please make sure all needed information is on the parameter server.
terminate called after throwing an instance of 'std::runtime_error'
  what():  Unable to construct robot model. Please make sure all needed information is on the parameter server.
[ros2run]: Aborted

Things I considered:

The parameters I set for the MoveGroupInterface might be wrong. I tried several combinations. But the namespace should be lbr and the planning_group should be arm.
I checked the ros2 param list and I see the robot_description being used by multiple nodes. I am a bit lost though on what exactly I need and where it should be published so I can use it with the MoveGroupInterface.

Could it be that the robot_description is not properly being loaded into the ROS parameter server? Unfortunately I do not quite understand how the launch file works. I found this in the move_group_launch.py:

# RViz
rviz = 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": LaunchConfiguration("sim")}],
    remappings=[
        ("display_planned_path", robot_name + "/display_planned_path"),
        ("joint_states", robot_name + "/joint_states"),
        ("monitored_planning_scene", robot_name + "/monitored_planning_scene"),
        ("robot_description", robot_name + "/robot_description"),
        ("robot_description_semantic", robot_name + "/robot_description_semantic"),
    ],
)

Maybe this also needs to be published somewhere else? I would appreciate any guidance on this.

mhubii commented 2 months ago

could you try to run your node in namespace lbr. maybe that helps. You can do this via

ros2 run my_package my_node --ros-args -r __ns:=/lbr

If you run your node from a launch file, there is a namespace argument to every node

node = Node(
    ...
   namespace="lbr",
)

Nicolai-98 commented 2 months ago

Thank you for the input. This helped! It now finds the robot:

run lbr_moveit lbr_moveit --ros-args -r __ns:=/lbr
[INFO] [1721214923.569955746] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 1.00808 seconds
[INFO] [1721214923.570008807] [moveit_robot_model.robot_model]: Loading robot model 'iiwa7'...
[INFO] [1721214923.570018007] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[WARN] [1721214923.651692203] [moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!

However there still seems to be a problem with the kinematics plugin.

mhubii commented 2 months ago

no worries, maybe you'll have to parse this file

https://github.com/lbr-stack/lbr_fri_ros2_stack/blob/humble/lbr_moveit_config/iiwa7_moveit_config/config/kinematics.yaml

Try

ros2 run lbr_moveit lbr_moveit --ros-args \
    -r __ns:=/lbr \
    --params-file `ros2 pkg prefix iiwa7_moveit_config`/share/iiwa7_moveit_config/config/kinematics.yaml

Note you can also parse parameters from a yaml file via launch files. E.g.

import os

from ament_index_python import get_package_share_directory
from launch_ros.actions import Node

config_path = os.path.join(
    get_package_share_directory("iiwa7_moveit_config"), "config", "kinematics.yaml"
)

my_node = Node(
    ...
    parameters=[config_path],
)

Please note that MoveIt might require other configs, too.

Nicolai-98 commented 2 months ago

Thank you. I tried

ros2 run lbr_moveit lbr_moveit --ros-args -r __ns:=/lbr --params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml`

The path to the file is correct but I get:

[ERROR] [1721721951.561096407] [rcl]: Failed to parse global arguments
terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
  what():  failed to initialize rcl: Couldn't parse params file: '--params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml'. Error: Cannot have a value before ros__parameters at line 2, at ./src/parse.c:793, at ./src/rcl/arguments.c:406

I also tried the approach using a launch file:

import os
from launch import LaunchDescription
from ament_index_python import get_package_share_directory
from launch_ros.actions import Node

def generate_launch_description():

    config_path = os.path.join(
        get_package_share_directory("iiwa7_moveit_config"), "config", "kinematics.yaml"
    )

    return LaunchDescription([
        Node(
            package='lbr_moveit',  
            executable='lbr_moveit',  
            name='lbr_moveit_node',  
            namespace="lbr",
            parameters=[config_path]
        ),
    ])

The results are identical:

[lbr_moveit-1] [ERROR] [1721723234.073546384] [rcl]: Failed to parse global arguments
[lbr_moveit-1] terminate called after throwing an instance of 'rclcpp::exceptions::RCLInvalidROSArgsError'
[lbr_moveit-1]   what():  failed to initialize rcl: Couldn't parse params file: '--params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml'. Error: Cannot have a value before ros__parameters at line 2, at ./src/parse.c:793, at ./src/rcl/arguments.c:406
[ERROR] [lbr_moveit-1]: process has died [pid 2973, exit code -6, cmd '/home/ros2_ws/install/lbr_moveit/lib/lbr_moveit/lbr_moveit --ros-args -r __node:=lbr_moveit_node --params-file /home/ros2_ws/install/iiwa7_moveit_config/share/iiwa7_moveit_config/config/kinematics.yaml'].

Since the error suggested that there cannot be a value before ros_parameters. I added rosparameters to the kinematics.yaml. Then the error changed to: `Error: There are no node names before rosparameters at line 1, at ./src/parse.c:629, at ./src/rcl/arguments.c:406`
This is probably not the right approach but might give insight into the problem.

mhubii commented 2 months ago

arg you are right, how annoying. This file is from MoveIt and not configured to be used as parameters file.

You'd have to add your node name, however, this isn't a good solution.

E.g.

lbr_moveit:
  ros__parameters:
    arm:
      kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
      kinematics_solver_search_resolution: 0.0050000000000000001
      kinematics_solver_timeout: 0.0050000000000000001

A slightly better approach would be to read the configs as opposed to parsing the path.

May I ask, do you have a fork of this repo that we could collaborate on?

Just wondering if there is a better way to use the MoveIt C++ API (haven't got experience either)

ahnwoohyeon commented 2 months ago

I had the same problem, so I followed the part where the name space was added as /lbr. However, the problem is that when I checked the action, the server was declared as /lbr/move_action, but when I run c++ code, it was declared as /move_action client, so it doesn't seem to have much response. How do I solve this?

mhubii commented 2 months ago

Putting this for reference:

https://github.com/moveit/moveit2_tutorials/blob/20be39797b17e460fda48975a0a8ee25f8ad87ac/doc/examples/moveit_cpp/launch/moveit_cpp_tutorial.launch.py#L10C5-L10C18

(configs are parsed to the node via moveit config builder)

Nicolai-98 commented 2 months ago

arg you are right, how annoying. This file is from MoveIt and not configured to be used as parameters file.

You'd have to add your node name, however, this isn't a good solution.

E.g.
lbr_moveit:
  ros__parameters:
    arm:
      kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
      kinematics_solver_search_resolution: 0.0050000000000000001
      kinematics_solver_timeout: 0.0050000000000000001
A slightly better approach would be to read the configs as opposed to parsing the path.

May I ask, do you have a fork of this repo that we could collaborate on?

Just wondering if there is a better way to use the MoveIt C++ API (haven't got experience either)

Ah seems I was close with the ros__parameters key. But I agree it's not an optimal solution. Regarding the fork: Currently all I have done is create a c++ ros2 package called lbr_moveit that contains node with

the hello world moveit content following the docs: https://moveit.picknik.ai/main/doc/tutorials/your_first_project/your_first_project.html
and the launch file for that node (posted above) in order to test out moveit with this stack.
I am also looking for an optimal way to achieve this. I can create a fork with these changes for easier collaboration tomorrow morning.

mhubii commented 2 months ago

There is now a demo, you can pull the latest changes from the humble branch.

You can find the doc here: https://lbr-stack.readthedocs.io/en/latest/lbr_fri_ros2_stack/lbr_demos/lbr_moveit_cpp/doc/lbr_moveit_cpp.html

The difficulty is that the MoveGroup for this repository lives inside a namespace.

ahnwoohyeon commented 2 months ago

When moving motion planning is conducted through rviz, it seems to be accessible normally. But why is it impossible to access it if it is conducted through c++?

lbr-stack / lbr_fri_ros2_stack

Move it #140