arm_kinematics_constraint_aware waiting for environment [closed]

edit

Hi all,

We are trying to implement a arm_kinematics_constraint_aware node, following this tutorial.

We get no errors messages, but no services are being published and the following gets printed periodically:

[INFO] Waiting for environment server planning scene registration service /register_planning_scene
[INFO] waitForService: Service [/register_planning_scene] has not been advertised, waiting...

Similar problems have been posted before (eg. here and here), and making sure JointState is being published for _all_ joints seem to solve the problem for most people. Unfortunately, not in our case...

Here's an overview of our setup:

ROS Fuerte with Ubuntu 12.04.

Parameters set on rosparam server in robot_description_planning namespace:

multi_dof_joints:
  - name: world_joint
    type: Fixed
    parent_frame_id: world
    child_frame_id: world

groups:
  - name: rbase
    joints:
      - base_joint
    links:
      - world
      - base_link

  - name: arm
    joints:
      - A1
      - A2
      - A3
      - A4
      - A5
      - A6
      - A6-TOOL
    links:
      - A1
      - A2
      - A3
      - A4
      - A5
      - A6
      - TOOL

default_collision_operations:
  - object1: TOOL
    object2: A1
    operation: enable

Parameters set in example_arm_kinematics namespace (example_kinematics.yaml):

arm:
  tip_name: TOOL
  root_name: base_link
  kinematics_solver: arm_kinematics_constraint_aware/KDLArmKinematicsPlugin

use_collision_map: false
collision_map_safety_timeout: 10000.0
joint_states_safety_timeout: 1.0
tf_safety_timeout: 10.0
default_robot_padding: 0.02
link_padding:
  - link: arm
    padding: .02

/robot_description is loaded using an urdf model.

The launch file we are using looks like this:

<launch>
    <node pkg="arm_kinematics_constraint_aware" type="arm_kinematics_constraint_aware" name="example_arm_kinematics" output="screen">
    <rosparam command="load" file="$(find collision_test)/example_kinematics.yaml" />
    </node>
</launch>

Following joints are being published in a JointState message (0 values for fixed joints):

["world_joint", "base_joint", "A1", "A2", "A3", "A4", "A5", "A6", "A6-TOOL"]

Any ideas would be greatly appreciated!

Thanks in advance,

Eli