Moveit: getCurrentJointValues() won't update

[唐威]

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

[唐威]

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.

在 2018年1月29日星期一 UTC下午5:10:30，唐威写道：