Add bhand/command publisher to wam_node_sim.

diff --git a/wam_node_sim/include/wam_node_sim/hand.h b/wam_node_sim/include/wam_node_sim/hand.h
index 5336fe3..d15fca7 100644 (file)
--- a/wam_node_sim/include/wam_node_sim/hand.h
+++ b/wam_node_sim/include/wam_node_sim/hand.h
@@ -76,7 +76,7 @@ public:
        static const unsigned int WHOLE_HAND = GRASP | SPREAD;
 
        /** Hand Constructor */
-       Hand(void);
+       Hand(const std::string &nodeName="bhand");
        /** Hand Destructor */
        ~Hand();
        /** initialize Method */
@@ -143,10 +143,11 @@ public:
        /** getTactilePucks Method creates container of pucks to get tactile sensor data from possible locations */
 //     const std::vector<TactilePuck*>& getTactilePucks() const { return tactilePucks; }
 
-       void jointStatesCB(const sensor_msgs::JointState &jointStates);
 
        static const size_t SPREAD_INDEX = 3;
-
+       
+       void jointStatesCB(const sensor_msgs::JointState &jointStates);
+       
 //protected:
 private:
        /** */
@@ -185,7 +186,8 @@ private:
        static const int CMD_OPEN  = 20;
 //     static const enum Puck::Property props[];
 
-       jp_type jointPosition_;
+       ros::Publisher commandPub_; // controller command
+
 
        DISALLOW_COPY_AND_ASSIGN(Hand);
 

diff --git a/wam_node_sim/launch/gazebo.launch b/wam_node_sim/launch/gazebo.launch
index 33d3f3f..a83f925 100644 (file)
--- a/wam_node_sim/launch/gazebo.launch
+++ b/wam_node_sim/launch/gazebo.launch
@@ -10,8 +10,9 @@
        <arg name="config" default="$(find wam_bringup)/config/$(arg controller).yaml"/>
 
        <group>
-               <remap from="/joint_states" to="gazebo/joint_states" />
+               <remap from="joint_states" to="gazebo/joint_states" />
                <remap from="wam/command" to="controller/command" />
+               <remap from="bhand_controller/command" to="bhand/command" />
 
                <include file="$(find wam_bringup)/launch/gazebo.launch" >
                        <arg name="paused" value="$(arg paused)"/>

diff --git a/wam_node_sim/src/hand.cpp b/wam_node_sim/src/hand.cpp
index dc93388..391e5be 100644 (file)
--- a/wam_node_sim/src/hand.cpp
+++ b/wam_node_sim/src/hand.cpp
@@ -45,6 +45,7 @@
 #include <limits>
 
 #include <ros/ros.h>
+#include <std_msgs/Float64MultiArray.h>
 
 #include <wam_node_sim/hand.h>
 
@@ -57,7 +58,7 @@ static const double SPREAD_COUNTS_PER_RAD=35840/M_PI;
 //const enum Puck::Property Hand::props[] = { Puck::HOLD, Puck::CMD, Puck::MODE, Puck::P, Puck::T, Puck::SG };
 
 /** Hand Constructor */
-Hand::Hand(void) :
+Hand::Hand(const std::string &nodeName) :
 //     MultiPuckProduct(DOF, _pucks, PuckGroup::BGRP_HAND, props, sizeof(props)/sizeof(props[0]), "Hand::Hand()"),
        hasFtt(false), hasTact(false), useSecondaryEncoders(true), encoderTmp(DOF), primaryEncoder(DOF, 0), secondaryEncoder(DOF, 0), ftt(DOF, 0) //, tactilePucks()
        {
@@ -104,11 +105,16 @@ Hand::Hand(void) :
 //     j2pt[SPREAD_INDEX] = motorPucks[SPREAD_INDEX].getIpnm() / SPREAD_RATIO;
 
        for(int i=0;i < DOF;i++) encoderTmp[i].resize(2);
+
+       ros::NodeHandle node_(nodeName); // bhand specific nodehandle   
+       commandPub_=node_.advertise<std_msgs::Float64MultiArray>("command", 1); // bhand/command
 }
 /** Hand Destructor */
 Hand::~Hand()
 {
 //     detail::purge(tactilePucks);
+       
+       commandPub_.shutdown();
 }
 /** initialize Method */
 void Hand::initialize() const