ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
1

MoveIt stop command usage

asked 2018-01-22 03:34:51 -0500

Lauran gravatar image

updated 2018-01-22 11:45:02 -0500

Hello,

I want to stop a UR5 while it is executing a movement. Now, I have three different scripts. One which sends information like coordinates and variables. One which makes the UR5 execute a movement and the third script should stop the robot. My question is now, how can I stop the robot. I found a command which should stop it. I've read on some forum that it should work with some delay.

void moveit::planning_interface::MoveGroup::stop (void)

So my question basically is: How do I implement this void in my code in such a way that I can make the UR5 stop. This is what I currently have.

#include <iostream>
#include <ros/ros.h>
#include <moveit/move_group_interface/move_group.h>
#include <sstream>
#include <ur5_inf3480/Coor.h>

class StopRobot {
public:
    int a;
    int counter;
    void stop(void);
    void PublishCoordinatesCallback(const ur5_inf3480::Coor msg);
};

void StopRobot::PublishCoordinatesCallback(const ur5_inf3480::Coor msg) {
    a = msg.a;
    counter = msg.counter;
}

void StopRobot::stop(void) {
    moveit::planning_interface::MoveGroup::stop();
  }

int main(int argc, char **argv) {
    ros::init(argc, argv, "coordinates");
    ros::NodeHandle nh;
    ros::Rate loop_rate(30);
    StopRobot sr;
    ros::Subscriber sub = nh.subscribe<ur5_inf3480::Coor>("coordinates", 1000, &StopRobot::PublishCoordinatesCallback, &sr);
    int a = sr.a;
    int counter = sr.counter;
    while (ros::ok()) {
        a = sr.a;
        loop_rate.sleep();
        ros::spinOnce();
        if (a == 0) {
            sr.stop();
        }
    }
return 0;
}

When I catkin_make the code like this it gives the following error.

/home/lauran/ur5robot_ws/src/ur5_inf3480/src/stop_robot.cpp: In member function ‘void StopRobot::stop()’:
/home/lauran/ur5robot_ws/src/ur5_inf3480/src/stop_robot.cpp:22:46: error: cannot call member function ‘void moveit::planning_interface::MoveGroup::stop()’ without object
  moveit::planning_interface::MoveGroup::stop();

I have tried to do different sorts of code but every code gives another error. So I'm a bit stuck now on how the usage of this certain command is. Could someone please help me on this.

Thank you.

edit retag flag offensive close merge delete

Comments

Can you please update your question with a copy and paste of the errors (not a screenshot)? This way we don't have to mentally compile the code to figure out what the error is. Thanks.

jayess gravatar image jayess  ( 2018-01-22 11:20:16 -0500 )edit

Yes of course, sorry for that.

Lauran gravatar image Lauran  ( 2018-01-22 11:41:47 -0500 )edit

1 Answer

Sort by » oldest newest most voted
3

answered 2018-01-23 03:36:42 -0500

updated 2018-01-24 08:58:56 -0500

You're trying to call the stop method as if it was static, you'll need to create a concrete instance of the MoveGroup object to be able to call the stop method. If you add a MoveGroup instance to the StopRobot class as shown below:

moveit::planning_interface::MoveGroup moveGroupInstance;

Then call it in the stop method like this:

moveGroupInstance.stop();

I'm not 100% sure that you can call this method from any node, it's possible you'll have to call this on the same MoveGroup object that's being used to execute the move. But give it a try and let us know how you get on.

Update : There are two different move commands in the moveit interface. asyncMove() returns instantly allowing your node to execute other code while the robot is moving. While the move() command is blocking, which means that execution of your node will be effectively paused until the robot has completed executing the plan.

If you use the asyncMove() command then you'll be able to receive messages and potentially stop the robot from moving while the arm is in motion.

edit flag offensive delete link more

Comments

When I catkin_make the adjustment I get many errors starting with:

error: no matching function for call to ‘moveit::planning_interface::MoveGroup::MoveGroup()’
  moveit::planning_interface::MoveGroup moveGroupInstance;

after that 4 errors saying

Lauran gravatar image Lauran  ( 2018-01-24 02:21:38 -0500 )edit

note: candidate expects 3 arguments, 0 provided

And then all four follow with this or something like this:

moveit::planning_interface::MoveGroup::MoveGroup(const string&, const boost::shared_ptr<tf::Transformer>&, const ros::WallDuration&)
   MoveGroup(const std::string& group
Lauran gravatar image Lauran  ( 2018-01-24 02:22:43 -0500 )edit

I had tried earlier to see what happens in my script that moves the UR5 whenever I move the UR5. I see that my script that moves the UR5 waits until the UR5 has moved. So I can't call a stop function while it moves. Therefor I had the idea to use different scripts. So I can call stop while it moves.

Lauran gravatar image Lauran  ( 2018-01-24 02:23:45 -0500 )edit

There are two different move commands in moveit. See my updated answer to explain how you can stop your robots from the same node.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-01-24 08:56:06 -0500 )edit

The error is solved! I gave the movegroup the wrong name. Now I have given it the same name as the movegroup that moves the UR5, at first I didn't and there was where the mistake came from. Now I move the UR5 through one script and stop it with another. So super happy about that thank you!!

Lauran gravatar image Lauran  ( 2018-01-25 06:03:49 -0500 )edit

Great. I'm glad you've got it working.

PeteBlackerThe3rd gravatar image PeteBlackerThe3rd  ( 2018-01-25 06:05:26 -0500 )edit

There still seems to be some delay before it actually stops though, this might be because I ros::init twice. In the move script and in the stop script. So I will also look into getting the stop void in the same script as the move void. I don't use move() but execute(). So I need to look into that!:)

Lauran gravatar image Lauran  ( 2018-01-25 06:07:16 -0500 )edit
3

I have also managed to implement stopping the UR5 while it moves when the stop void is within the same script as where the UR5 is told to move. Just saying for the next person who might have the same issue. Feel free to contact me for the code ;).

Lauran gravatar image Lauran  ( 2018-01-25 12:52:06 -0500 )edit

Question Tools

1 follower

Stats

Asked: 2018-01-22 03:34:51 -0500

Seen: 3,125 times

Last updated: Jan 24 '18