Ask Your Question
0

Moveit: getCurrentJointValues() won't update

asked 2018-01-26 18:51:18 -0500

weitang gravatar image

Setup: ubuntu 14.04 LTS, ROS indigo, moveit, youbot in vrep simulator Problem Description: I have a node from vrep publishing to topic /joint_states. Then I run the demo.launch from config pkg, commenting out the joint_state_publisher node. Finally, when I test my own code, calling the getCurrentJointValues() function, the returned joint values stay the default values not updated.

The code I write is very similar to Move Group Interface Tutorial.

moveit::planning_interface::MoveGroup group("youbot_arm");
moveit::planning_interface::MoveGroup::Plan my_plan;
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
std::vector<double> group_variable_values;
group.startStateMonitor();
group_variable_values = group.getCurrentJointValues();
while(1){ 
    ROS_INFO_STREAM("current joint value:");
    for(int k = 0;k < group_variable_values.size();k++){
        ROS_INFO_STREAM(group_variable_values[k]<<",");
    }
    sleep(3.0);
}

From the output of rostopic echo /joint_states, I get the joint position for "arm_joint_1". "arm_joint_2", "arm_joint_3", "arm_joint_4", "arm_joint_5" are 0.8, 0, 0, 0, 0. However, I get 2.94961, 1.35263, -2.59181, 0, 0 from my program, which is a default value I believe. Also, the rviz bring up by demo.launch indeed can show the correct current position of the robot, can the gui planning module works just fine. I just can't access the current state from code.

Can anyone tell me any possible directions I should go? Thank you very much!

wei

edit retag flag offensive close merge delete

Comments

I don't think you can use demo.launch in the way you describe. Can you clarify what you changed exactly?

gvdhoorn gravatar imagegvdhoorn ( 2018-01-27 05:09:29 -0500 )edit

In the demo.launch, since there is no real robot, so there is this fake joint state publisher, publishing to /joint_states. I already have a node publishing this message, so I delete this fake joint state publisher.

weitang gravatar imageweitang ( 2018-01-27 08:51:47 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2018-01-30 13:34:15 -0500

weitang gravatar image

I solved the issue. It may not be the same situation for everyone, But I post it here for future reference. I compiled moveit from source and run the same program, first, it shows: "Failed to fetch the current state" Then I did the following change, In moveit/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp

I add a ros::spinOnce() at line 273 within the waitForCurrentState() function, because it seems the problem is the joint state callback function never get called. After I did this change, it works and can correctly get the current joint values.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2018-01-26 18:51:18 -0500

Seen: 439 times

Last updated: Jan 30 '18