NaNs in /joint_states when using joint_trajectory_controller in namespace

edit

Hello all,

Short Version:

I have a Husky and a Kinova arm which I'd like to run under a namespace. This block of code results in a correctly spawned Husky with an arm (albeit uncontrolled) in Gazebo:

<node name="robot_controller_spawner" pkg="controller_manager" type="spawner"
       args="husky_joint_publisher
                  husky_velocity_controller"/>

When I add joint_trajectory_controller to the spawner, /joint_states publishes NaNs, and the robot and all joints get moved to the origin.

<node name="robot_controller_spawner" pkg="controller_manager" type="spawner"
      args="husky_joint_publisher
            husky_velocity_controller 
            effort_joint_trajectory_controller
            effort_finger_trajectory_controller"/>

Long Version:

I am trying to set up an environment with two Huskies, each with a Kinova arm for ROS Kinetic. I've managed to modify the Husky launch files, xacros, and yaml files to work with namespaces using nre_summultihusky for Indigo as the blueprint for Kinetic. Now, I am trying to set up the Kinova arm for use with namespaces.

The Kinova arm is controlled with joint_trajectory_controller, which does not work with namespaces. So, I modified the source as described here so that a namespace can be prepended to robot_description.

However, when I try to run, something is failing. All movable joints are spawning at the origin. It looks like this:

If you were to look under the map you would see the arm and wheels all smashed together under the vehicle at the origin. There are no errors, as you will see in the below print out.

These lines were modified in j2n6s300.urdf.xacro

<xacro:macro name="j2n6s300" params="base_parent nsp prefix:=j2n6s300">

    <xacro:gazebo_config robot_namespace="/${nsp}/${prefix}"/>

Here is my control.launch:

<?xml version="1.0"?>
<launch>

  <arg name="namespace" default="h0"/>
  <arg name="tfpre" default="$(arg namespace)_tf" />
  <arg name="x" default="0.0"/>
  <arg name="y" default="0.0"/>
  <arg name="yaw" default="0.0"/>

  <arg name="kinova_robotType" default="j2n6s300"/>
  <arg name="kinova_robotName" default="$(arg kinova_robotType)"/> 

  <arg name="config_extras"
       default="$(eval optenv('HUSKY_CONFIG_EXTRAS', find('husky_control') + '/config/empty.yaml'))"/>

  <arg name="laser_enabled" default="$(optenv HUSKY_LMS1XX_ENABLED false)"/>
  <arg name="kinect_enabled" default="$(optenv HUSKY_KINECT_ENABLED false)"/>
  <arg name="urdf_extras" default="$(optenv HUSKY_URDF_EXTRAS)"/>

  <!-- Load robot description -->
  <include file="$(find husky_description)/launch/description.launch" >
    <arg name="namespace" default="$(arg namespace)"/>
    <arg name="tfpre" default="$(arg namespace)_tf" />
    <arg name="laser_enabled" default="$(arg laser_enabled)"/>
    <arg name="kinect_enabled" default="$(arg kinect_enabled)"/>
    <arg name="urdf_extras" default="$(arg urdf_extras)"/>
  </include>

  <!-- Load controller configuration -->
  <rosparam command="load" file="$(find husky_control)/config/control.yaml" />
  <rosparam file="$(find kinova_control)/config/$(arg kinova_robotName)_control.yaml" command="load"/>

  <rosparam param="husky_velocity_controller/base_frame_id" subst_value="True">$(arg tfpre)/base_link</rosparam>

  <!-- Apply namespace to robot_description for controllers -->
  <rosparam param="effort_finger_trajectory_controller/robot_description" subst_value="True">/$(arg namespace)/robot_description</rosparam>  
  <rosparam param="effort_joint_trajectory_controller/robot_description" subst_value="True">/$(arg namespace)/robot_description</rosparam>

  <!-- Spawn controllers -->

  <node name="robot_controller_spawner" pkg="controller_manager" type="spawner"
        args="husky_joint_publisher
              husky_velocity_controller 
              effort_joint_trajectory_controller
              effort_finger_trajectory_controller"/>

  <!-- Start EKF for localization -->
  <node pkg="robot_localization" type="ekf_localization_node" name="ekf_localization">
    <rosparam command="load" file="$(find husky_control)/config/localization.yaml" />
  </node>

  <node pkg="tf" type="static_transform_publisher" name="map_odom_broadcaster" args="$(arg x) $(arg y) 0 $(arg yaw) 0 0 /map /$(arg tfpre)/odom 100" />

  <node pkg="interactive_marker_twist_server" type="marker_server" name="twist_marker_server ...