Ask Your Question
0

constraint_aware_simple_arm_server hanging on waitForService environment server at startup

asked 2011-10-21 02:45:39 -0500

Pi Robot gravatar image

updated 2011-10-21 02:46:37 -0500

Hello,

I have been using the simple_arm_server on my TurtleBot arm with great success. Just now I tried the constraint aware version using the command:

$ roslaunch turtlebot_arm_bringup constraint_aware_simple_arm_server.launch

and the launch process gets stuck on connecting to the environment planning server:

process[robot_state_publisher-1]: started with pid [3356]
process[arbotix-2]: started with pid [3357]
process[gripper_controller-3]: started with pid [3358]
process[environment_server-4]: started with pid [3359]
process[planning_scene_validity_server-5]: started with pid [3360]
process[arm_kinematics-6]: started with pid [3362]
process[simple_arm_server-7]: started with pid [3368]
process[reset_arm_server-8]: started with pid [3370]
process[relax_arm_server-9]: started with pid [3372]
process[turtlebot_arm_marker_server-10]: started with pid [3378]
process[point_head_trajectory_server-11]: started with pid [3380]
[INFO] [WallTime: 1319208095.504630] ArbotiX being simulated.
[ INFO] [1319208095.608180070]: Using kinematics solver name: arm_kinematics_constraint_aware/KDLArmKinematicsPlugin
[ INFO] [1319208095.634567514]: Private handle registered under /arm_kinematics/arm
[ INFO] [1319208095.661157622]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1319208095.802252195]: Robot frame is 'base_footprint'
[ INFO] [1319208095.810133088]: Waiting for robot state ...
[ INFO] [1319208095.810199214]: Waiting for joint state ...
[INFO] [WallTime: 1319208095.868426] Started FollowController (head_controller). Joints: ['head_pan_joint', 'head_tilt_joint'] on C0
[INFO] [WallTime: 1319208095.876147] Started FollowController (arm_controller). Joints: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_flex_joint', 'wrist_flex_joint'] on C1
[ INFO] [1319208095.959457442]: [turtlebot arm marker server] Initialized.
[ WARN] [1319208095.980131019]: Got joint state update but did not update some joints for more than 1 second.  Turn on DEBUG for more info
[ INFO] [1319208096.007092525]: adding joint: [wrist_flex_joint]
[ INFO] [1319208096.007172558]: adding joint: [elbow_flex_joint]
[ INFO] [1319208096.007200572]: adding joint: [shoulder_lift_joint]
[ INFO] [1319208096.007223647]: adding joint: [shoulder_pan_joint]
[ INFO] [1319208096.007286585]: getting bounds for joint: [wrist_flex_joint]
[ INFO] [1319208096.007328744]: getting bounds for joint: [elbow_flex_joint]
[ INFO] [1319208096.007372557]: getting bounds for joint: [shoulder_lift_joint]
[ INFO] [1319208096.007406374]: getting bounds for joint: [shoulder_pan_joint]
[INFO] [WallTime: 1319208096.183245] point_head_server node started
[ INFO] [1319208096.357998012]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1319208096.674407888]: Waiting for environment server planning scene registration service /register_planning_scene
[ INFO] [1319208096.675806679]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1319208097.370046947]: Waiting for environment server planning scene registration service /register_planning_scene
[ INFO] [1319208097.371684377]: waitForService: Service [/register_planning_scene] has not been advertised, waiting..
etc.

I do not have the same problem when using the non-constraint aware version:

$ roslaunch turtlebot_arm_bringup simple_arm_server.launch

process[robot_state_publisher-1]: started with pid [5584]
process[arbotix-2]: started with pid [5585]
process[gripper_controller-3]: started with pid [5586]
process[environment_server-4]: started with pid [5587]
process[planning_scene_validity_server-5]: started with pid [5588]
process[arm_kinematics-6]: started with pid [5596]
process[simple_arm_server-7]: started with pid [5600]
process[reset_arm_server-8]: started with pid [5602]
process[relax_arm_server-9]: started with pid [5604]
process[turtlebot_arm_marker_server-10]: started with pid [5606]
process[point_head_trajectory_server-11]: started with pid [5607]
[ INFO] [1319208242.303636479]: waitForService: Service [/register_planning_scene] has not been advertised, waiting...
[ INFO] [1319208242.369045172]: Environment server started
[ INFO] [1319208242.373578908]: waitForService: Service [/register_planning_scene] is now available.
[INFO] [WallTime: 1319208242.420583] ArbotiX being simulated.
[INFO] [WallTime: 1319208242.615559] Started FollowController (head_controller). Joints: ['head_pan_joint', 'head_tilt_joint'] on C0
[ INFO] [1319208242.615987770]: Successfully connected to planning scene action server for /planning_scene_validity_server
[INFO] [WallTime: 1319208242.630950] Started FollowController (arm_controller). Joints: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_flex_joint', 'wrist_flex_joint'] on C1
[ INFO] [1319208242 ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2011-10-21 06:43:25 -0500

fergs gravatar image

Patrick, it looks as though you aren't running the turtlebot_arm_bringup/fake_pub.py in your simulation, please take a look at the "fake-arm.launch" file in turtlebot_arm_bringup, it has a line with:

  <node pkg="turtlebot_arm_bringup" type="fake_pub.py" name="fake_pub" />

You need to be publishing ALL joints or the planning environment won't start. You'll also need this on the real robot if you aren't actually connecting the iRobot Create.

edit flag offensive delete link more

Comments

That was it! Don't know why I disabled the fake_pub.py node at some point but after turning it back on, everything is working as advertised. Thanks!
Pi Robot gravatar imagePi Robot ( 2011-10-21 07:01:34 -0500 )edit
I just remembered why I disabled the fake_pub.py node at some point: this node publishes the joint states of the two TurtleBot wheels and castors for when you are not running the TurtleBot base. However, since it is publishing to the joint_state topic, I think it is clobbering the servo joint state message. Or to put it another way, the two messages alternate--one for the wheels/castors then one for servos, depending on the publishing rates of the ArbotiX and the fake_pub node. This means that my servo joint states were alternating between their actual values and all 0's which was messing up my head tracking node that depends on reading the servo joint states. Does that make sense?
Pi Robot gravatar imagePi Robot ( 2011-10-24 04:04:30 -0500 )edit
That sounds like your code needs to be improved. You should only update the joint state IF you find it in the message. Most ROS tools (such as robot_state_publisher) have been updated to handle joint_states from multiple locations.
fergs gravatar imagefergs ( 2011-10-24 05:23:09 -0500 )edit
Ah, that's good to know. I didn't realize that robot_state_publisher could handle joint_states from multiple nodes.
Pi Robot gravatar imagePi Robot ( 2011-10-24 05:30:44 -0500 )edit

Your Answer

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

Add Answer

Question Tools

Stats

Asked: 2011-10-21 02:45:39 -0500

Seen: 642 times

Last updated: Oct 21 '11