From: Walter Fetter Lages <w.fetter@ieee.org>
Date: Sun, 9 Dec 2018 00:24:19 +0000 (-0200)
Subject: Add bhand/command publisher to wam_node_sim.
X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=233377a723a89bbc2a4af0927435619c360b01e4;p=ufrgs_wam.git

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
--- 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
--- 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
--- 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