search

ROBOTIS-GIT / turtlebot3

ROS packages for Turtlebot3
http://turtlebot3.robotis.com
Apache License 2.0
1.5k stars 1.03k forks source link

Turtlebot3 with OM-X moveit move() doesn't wait for trajectory to complete before sending another one. #746

Open slavenpetkovic opened 3 years ago

slavenpetkovic commented 3 years ago

ISSUE TEMPLATE ver. 0.4.0

  1. Which TurtleBot3 platform do you use?

    • [ ] Burger
    • [ ] Waffle
    • [x] Waffle Pi

  2. Which ROS is working with TurtleBot3?

    • [ ] ROS 1 Kinetic Kame
    • [x] ROS 1 Melodic Morenia
    • [ ] ROS 1 Noetic Ninjemys
    • [ ] ROS 2 Dashing Diademata
    • [ ] ROS 2 Eloquent Elusor
    • [ ] ROS 2 Foxy Fitzroy
    • [ ] etc (Please specify your ROS Version here)

  3. Which SBC(Single Board Computer) is working on TurtleBot3?

    • [ ] Intel Joule 570x
    • [x] Raspberry Pi 3B+
    • [ ] Raspberry Pi 4
    • [ ] etc (Please specify your SBC here)

  4. Which OS you installed on SBC?

    • [ ] Raspbian distributed by ROBOTIS
    • [ ] Ubuntu MATE (16.04/18.04/20.04)
    • [x] Ubuntu preinstalled server (18.04/20.04)
    • [ ] etc (Please specify your OS here)

  5. Which OS you installed on Remote PC?

    • [ ] Ubuntu 16.04 LTS (Xenial Xerus)
    • [x] Ubuntu 18.04 LTS (Bionic Beaver)
    • [ ] Ubuntu 20.04 LTS (Focal Fossa)
    • [ ] Windows 10
    • [ ] MAC OS X (Specify version)
    • [ ] etc (Please specify your OS here)

  6. Specify the software and firmware version(Can be found from Bringup messages)

    • Software version: [x.x.x]
    • Firmware version: [x.x.x]

  7. Specify the commands or instructions to reproduce the issue.

    • HERE

  8. Copy and Paste the error messages on terminal.

    • HERE

  9. Please describe the issue in detail.

    • While using turtlebot3 with OM-X for pick and place function, I used MoveGroupInterface to move arm to desired position using this move_arm function. First I tried with plan + move() and with plan + execute functions and in main i called that move_arm function a few times to move arm to position1 then to position2 etc.. But it doesn't wait for first trajectory to complete before sending another one. The definition of that move() function says that it should wait for trajectory to complete.
    • Then i tried to monitor what is going on with /move_group/feedback and /move_group/status topics and it turns out that robot publishes that the arm reached goal state before it actually reached it. (status = 3 or something like that immediately after giving command to move).

//Move to valid NAMED pose (used for arm) void move_arm(moveit::planning_interface::MoveGroupInterface& move_group_interface, const std::string &name){ ros::NodeHandle node_handle; move_group_interface.setNamedTarget(name); moveit::planning_interface::MoveGroupInterface::Plan my_plan; bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO_NAMED("tutorial", "Planning (pose goal) %s", success ? "OK" : "FAILED"); bool result = (move_group_interface.execute(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO_NAMED("tutorial", "MOVE_GROUP_RESULT : %s", result ? "SUCCESS" : "FAILED"); }

In main is something like this

ros::AsyncSpinner spinner(1); spinner.start();

...

static const std::string PLANNING_GROUP_ARM = "arm"; moveit::planning_interface::MoveGroupInterface move_group_interface_arm(PLANNING_GROUP_ARM); move_group_interface_arm.setPlannerId("RRTConnect"); move_group_interface_arm.setPlanningTime(5); move_group_interface_arm.setNumPlanningAttempts(10);
const moveit::core::JointModelGroup* joint_model_group = move_group_interface_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM);

move_arm(move_group_interface_arm, "position1"); move_arm(move_group_interface_arm, "position2"); ...

When i add ros::WallDuration(5.0).sleep(); line in between these two move_arm calls, it works, but that is not the right way to do it.

Any idea why this happes or suggestion how to solve this problem? Is there anything else I should pass to MoveGroupInterface constructor except that planning group name string?

ROBOTIS-Will commented 3 years ago

Hi @slavenpetkovic How are you controlling the OM-X? Is it mounted on TB3 and connected to the OpenCR? Which ROS package are you using to control the OM-X? If you are implementing your code directly using the move group interface, you can inquire to the moveit repository below. https://github.com/ros-planning/moveit

slavenpetkovic commented 3 years ago

Yes, I use OpenCR to control TB3 with OM-X. And yes, I use moveit so i'll ask there. Thanks