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 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
↧