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 don't think you can use
demo.launchin the way you describe. Can you clarify what you changed exactly?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.