add joint_state_publisher plugin

[k-okada]

hi, I'm not sure what is the right way, but I think we can agree that we need to publish joint_state for robots (see https://github.com/davetcoleman/clam/blob/indigo-devel/clam_controller/src/joint_state_aggregator.cpp for example)
This PR is to create joint_state_publisher controller/plugin for dynamixel manager, so that you can spawn this by

    <node name="dynamixel_trajectory_controller_spawner_for_joint_state" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port=usb
                --type=meta
                joint_state_publisher
                rleg_j1_controller
                rleg_j2_controller
                rleg_j3_controller

of course you need yaml file such as

joint_state_publisher:
   controller:
       package: dynamixel_controllers
       module: joint_state_publisher
       type: JointStatePublisher

[arebgun]

I would prefer this to be a separate node, not in the framework of joint controllers. Something similar to this node: https://code.google.com/p/ua-ros-pkg/source/browse/trunk/arrg/ua_apps/wubble2_robot/nodes/joint_states_publisher.py but generalized, where you can specify joints in config file and just start the node as a regular ROS node. What do you think?

[k-okada]

I choose joint controller framework because we do not care about i/o part
in state publisher. I'm not sure how easy to reuse i/o part in standalone
ROS node, but having same framework may better in terms of re-usability and
maintenance.

◉ Kei Okada

On Tue, Feb 24, 2015 at 11:14 AM, Antons Rebguns [email protected]
wrote:

I would prefer this to be a separate node, not in the framework of joint
controllers. Something similar to this node:
https://code.google.com/p/ua-ros-pkg/source/browse/trunk/arrg/ua_apps/wubble2_robot/nodes/joint_states_publisher.py
but generalized, where you can specify joints in config file and just start
the node as a regular ROS node. What do you think?

—
Reply to this email directly or view it on GitHub
#27 (comment)
.

[130s]

+1 to @k-okada

Also, with a lack of any package in dynamixel_motor package suite that could be more suitable as the one @arebgun referred to (ua_apps), including the suggested change in this package (dynamixel_controllers) makes the best practical sense to me.

[k-okada]

updated. more error handling.

[dbolkensteyn]

this duplicates some of the logic already contained in the actual underlying controllers.

it might be better to subscribe to the /.../state topics of the underlying controllers and republish that data in the sensor_msgs/JointState format

as a quick hack, I've added /joint_state as a 2nd published topic by the meta controller, which publishes control_msgs/FollowJointTrajectoryFeedback whole actual field contains exactly all that's needed by sensor_msgs/JointState. I'll send out a PR with that in case anyone else is interested.

[k-okada]

Since I'm away from this PR for a year and not remember well what I did. To be clear, this node read data from servo motor via physical serial/usb interface, but you're suggesting we can get same information from control_msgs/FollowJointTrajectoryFeedback of joint_trajectory_action_controller. If so, my concern is that can we assume we always run that action server? of course that is the default interface for most of application, but for examplehttp://wiki.ros.org/dynamixel_controllers/Tutorials/CreatingJointPositionController tutorials is not assuming that action server, so I think this cause confusions.

by the way, speaking of joint trajectory action, that does publish feedback only when the input trajectory command is interpolated, I think most of other robot publish feedback information all the time, so we need to fix somthing like this ->
def update_state(self): in joint_trajectory_action_controller.py

                 self.msg.error.velocities[i] = self.msg.actual.velocities[i] - self.msg.desired.velocities[i]
                  
              self.state_pub.publish(self.msg)
 +            # pusblish feedback
 +            if self.action_server.is_active():
 +                self.msg.desired.time_from_start = self.msg.actual.time_from_start = self.msg.error.time_from_start = self.msg.header.stamp - self.action_server.current_goal.get_goal().trajectory.header.stamp
 +                self.action_server.publish_feedback(self.msg)
              rate.sleep()