Ask Your Question
2

Moveit move_group_interface freezes after planning towards current pose [closed]

asked 2016-07-15 07:46:42 -0600

Marcel Usai gravatar image

updated 2016-08-16 09:56:44 -0600

Hi,

When planning towards a pose goal with move_group_interface, my program freezes. The goal was successfully planned, there is no error message:

[ INFO] [1468585702.343711063]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1468585702.346383724]: No planner specified. Using default.
[ INFO] [1468585702.347143174]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1468585702.386972714]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1468585702.387030791]: Solution found in 0.040406 seconds
[ INFO] [1468585702.394637573]: SimpleSetup: Path simplification took 0.001726 seconds and changed from 3 to 2 states

After the last message, the program freezes.

To plan the pose goal, I used the following code:

this->move_group_interface->setPoseTarget(this->move_group_interface->getCurrentPose(this->move_group_interface->getEndEffectorLink()), this->move_group_interface->getEndEffectorLink());

I cannot imagine, why the moveit::planning_interface::MoveGroup::plan() should block my program even after the plan was successfully created.

Thank you for your help!


Update: The same behaviour is observed for moveit::planning_interface::MoveGroup::move() after setting a pose goal and a (valid) position near the current position:

geometry_msgs::PoseStamped pose_s = this->move_group_interface->getCurrentPose(this->move_group_interface->getEndEffectorLink());
pose_s.pose.position.x += 0.1;
this->move_group_interface->setPoseTarget(pose_s, this->move_group_interface->getEndEffectorLink());
moveit::planning_interface::MoveItErrorCode success = this->move_group_interface->move();

Program output looks similar (only addition is, the robot in RVIZ is moved to show the calculating path. Then, my program freezes, like before):

[ INFO] [1468846527.748218369]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1468846527.748530839]: Planning attempt 1 of at most 1
[ INFO] [1468846527.751398850]: No planner specified. Using default.
[ INFO] [1468846527.752396733]: RRTConnect: Starting planning with 1 states already in datastructure
[ INFO] [1468846527.795039078]: RRTConnect: Created 5 states (3 start + 2 goal)
[ INFO] [1468846527.795096722]: Solution found in 0.043326 seconds
[ INFO] [1468846527.808806888]: SimpleSetup: Path simplification took 0.000047 seconds and changed from 4 to 2 states
[ INFO] [1468846527.813859153]: Fake execution of trajectory

The robot is moved. The moveit::planning_interface::MoveGroup::move() function also blocks my code forever. Seems like it waits for the (non-existent, as I do not use fake execution) action server for an unlimited amount of time to respond. Gdb shows that the program freezes here:

Single stepping until exit from function _ZNK3ros10NodeHandle2okEv,
which has no line number information.
0x00007f90874c33a7 in actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction_<std::allocator<void> > >::waitForResult(ros::Duration const&) () from /opt/ros/indigo/lib/libmoveit_move_group_interface.so
edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by Marcel Usai
close date 2016-08-29 06:19:02.762938

Comments

Could it be that MoveIt is not receiving any joint state msgs / updates, and thus cannot properly track the state of execution of your trajectory? Hanging in waitForResult(..) and your comment "the robot is moved" seem to point in that direction.

gvdhoorn gravatar imagegvdhoorn ( 2016-07-18 11:38:21 -0600 )edit

Sorry for my late answer, but this can't be the case, because when I move the robot using joint space, RVIZ follows properly. Also the robot is not moving, just the RVIZ sim shows the 'preview' of the computed path, then my program freezes because of move_group_interface. updated description on this

Marcel Usai gravatar imageMarcel Usai ( 2016-08-16 09:55:34 -0600 )edit

Could it be that something cannot be initialized? (Referring to: this) I call plan in my main loop

Marcel Usai gravatar imageMarcel Usai ( 2016-08-16 12:53:53 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted
3

answered 2016-08-26 06:28:57 -0600

Marcel Usai gravatar image

I had to put the whole loop that calls move_groupe_interface's plan function in a new thread for it to return.

(I followed the this discussion and, as stated there, disattached the call to plan from my main loop)

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2016-07-15 07:46:42 -0600

Seen: 757 times

Last updated: Aug 26 '16