From b3215d9da5ec7071467563d20cdb07ac0e7f420c Mon Sep 17 00:00:00 2001
From: Sergi Hernandez Juan <shernand@iri.upc.edu>
Date: Wed, 30 Aug 2017 13:48:01 +0200
Subject: [PATCH] Added attributes and functions in the driver to be used in
the nodelet version (the parameters for all the nodes are the same). Changed
the NodeHandle used to create topics in different namespaces. Changed the
parameters of the launch files.
---
.../include/ir_foot_sensor_driver.h | 6 +++++
.../include/ir_foot_sensor_driver_node.h | 1 +
ir_foot_sensor/launch/darwin_left_foot.launch | 4 ++--
.../launch/darwin_right_foot.launch | 4 ++--
.../launch/darwin_two_feet_nodelet.launch | 8 +++----
ir_foot_sensor/src/ir_foot_sensor_driver.cpp | 23 ++++++++++++++++++-
.../src/ir_foot_sensor_driver_node.cpp | 16 ++++++++++---
7 files changed, 50 insertions(+), 12 deletions(-)
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver.h b/ir_foot_sensor/include/ir_foot_sensor_driver.h
index abed4b7..e459dde 100644
--- a/ir_foot_sensor/include/ir_foot_sensor_driver.h
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver.h
@@ -56,6 +56,9 @@ class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
private:
// private attributes and methods
CIRFeet<CDynamixelServerSerial> *device;
+ std::string dyn_device;
+ unsigned int dyn_baudrate;
+ unsigned char dev_id;
public:
/**
* \brief define config type
@@ -150,6 +153,9 @@ class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
std::vector<double> get_all_sensor_thresholds(void);
void set_sensor_threshold(adc_dma_ch_t channel_id, double threshold);
std::vector<bool> get_all_sensor_status(void);
+ void set_dynamixel_device(std::string &device);
+ void set_dynamixel_baudrate(unsigned int baudrate);
+ void set_device_id(unsigned char dev_id);
/**
* \brief Destructor
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver_node.h b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
index b93c024..1b28e08 100644
--- a/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
@@ -187,6 +187,7 @@ class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootS
class IrFootSensorDriverNodelet : public nodelet::Nodelet
{
private:
+ ros::NodeHandle nodelet_nh;
IrFootSensorDriverNode *node;
virtual void onInit();// initialization function
// thread attributes
diff --git a/ir_foot_sensor/launch/darwin_left_foot.launch b/ir_foot_sensor/launch/darwin_left_foot.launch
index 61d8490..3e25782 100644
--- a/ir_foot_sensor/launch/darwin_left_foot.launch
+++ b/ir_foot_sensor/launch/darwin_left_foot.launch
@@ -4,8 +4,8 @@
pkg="ir_foot_sensor"
type="ir_foot_sensor"
output="screen">
- <param name="dyn_serial" value="A4008atn"/>
- <param name="dyn_baudrate" value="1000000"/>
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
<param name="ir_foot_id" value="1"/>
<remap from="/darwin_left_foot/sensor_data"
to="/darwin/sensors/left_foot_data"/>
diff --git a/ir_foot_sensor/launch/darwin_right_foot.launch b/ir_foot_sensor/launch/darwin_right_foot.launch
index df674f2..51cd6f3 100644
--- a/ir_foot_sensor/launch/darwin_right_foot.launch
+++ b/ir_foot_sensor/launch/darwin_right_foot.launch
@@ -4,8 +4,8 @@
pkg="ir_foot_sensor"
type="ir_foot_sensor"
output="screen">
- <param name="dyn_serial" value="A4008atn"/>
- <param name="dyn_baudrate" value="1000000"/>
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
<param name="ir_foot_id" value="2"/>
<remap from="/darwin_right_foot/sensor_data"
to="/darwin/sensors/right_foot_data"/>
diff --git a/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
index 210fb48..7146cd7 100755
--- a/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
+++ b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
@@ -13,8 +13,8 @@
name="darwin_left_foot"
args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
output="screen">
- <param name="dyn_serial" value="A4008atn"/>
- <param name="dyn_baudrate" value="1000000"/>
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
<param name="ir_foot_id" value="1"/>
<remap from="/darwin_left_foot/sensor_data"
to="/darwin/sensors/left_foot_data"/>
@@ -26,8 +26,8 @@
name="darwin_right_foot"
args="load ir_foot_sensor/IrFootSensorDriverNodelet dynamixel_devices"
output="screen">
- <param name="dyn_serial" value="A4008atn"/>
- <param name="dyn_baudrate" value="1000000"/>
+ <param name="dyn_serial" value="/dev/ttyUSB0"/>
+ <param name="dyn_baudrate" value="115200"/>
<param name="ir_foot_id" value="2"/>
<remap from="/darwin_right_foot/sensor_data"
to="/darwin/sensors/right_foot_data"/>
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
index 7d71cff..18fdc9b 100644
--- a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
@@ -4,6 +4,9 @@ IrFootSensorDriver::IrFootSensorDriver(void)
{
//setDriverId(driver string id);
this->device=NULL;
+ this->dyn_device="";
+ this->dyn_baudrate=-1;
+ this->dev_id=-1;
}
bool IrFootSensorDriver::openDriver(void)
@@ -17,7 +20,10 @@ bool IrFootSensorDriver::openDriver(void)
this->device=NULL;
}
name << "foot_" << this->config_.ir_foot_id;
- this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
+ if(this->dyn_device!="" && this->dyn_baudrate!=-1 && this->dev_id!=-1)
+ this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->dyn_device,this->dyn_baudrate,this->dev_id);
+ else
+ this->device=new CIRFeet<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
return true;
}catch(CException &e){
@@ -105,6 +111,21 @@ std::vector<bool> IrFootSensorDriver::get_all_sensor_status(void)
return std::vector<bool>();
}
+void IrFootSensorDriver::set_dynamixel_device(std::string &device)
+{
+ this->dyn_device=device;
+}
+
+void IrFootSensorDriver::set_dynamixel_baudrate(unsigned int baudrate)
+{
+ this->dyn_baudrate=baudrate;
+}
+
+void IrFootSensorDriver::set_device_id(unsigned char dev_id)
+{
+ this->dev_id=dev_id;
+}
+
IrFootSensorDriver::~IrFootSensorDriver(void)
{
if(this->device!=NULL)
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
index a3773e6..1dcea82 100644
--- a/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
@@ -3,11 +3,13 @@
IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
{
+ std::string dyn_serial="";
+ int dyn_baudrate=-1,dev_id=-1;
//init class attributes if necessary
this->loop_rate_ = 10;//in [Hz]
// [init publishers]
- this->sensor_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
+ this->sensor_data_publisher_ = this->node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
// [init subscribers]
@@ -29,6 +31,13 @@ IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
this->sensor_data_ir_foot_data_msg_.names[7]="front_left";
this->sensor_data_ir_foot_data_msg_.names[8]="front_right";
this->sensor_data_ir_foot_data_msg_.names[9]="front_analog";
+
+ this->node_handle_.getParam("dyn_serial",dyn_serial);
+ this->driver_.set_dynamixel_device(dyn_serial);
+ this->node_handle_.getParam("dyn_baudrate",dyn_baudrate);
+ this->driver_.set_dynamixel_baudrate(dyn_baudrate);
+ this->node_handle_.getParam("ir_foot_id",dev_id);
+ this->driver_.set_device_id(dev_id);
}
void IrFootSensorDriverNode::mainNodeThread(void)
@@ -111,7 +120,7 @@ int main(int argc,char *argv[])
#include <pluginlib/class_list_macros.h>
-IrFootSensorDriverNodelet::IrFootSensorDriverNodelet()
+IrFootSensorDriverNodelet::IrFootSensorDriverNodelet()
{
this->node=NULL;
}
@@ -131,7 +140,8 @@ IrFootSensorDriverNodelet::~IrFootSensorDriverNodelet(void)
void IrFootSensorDriverNodelet::onInit()
{
// initialize the driver node
- this->node=new IrFootSensorDriverNode(getPrivateNodeHandle());
+ this->nodelet_nh=ros::NodeHandle(getPrivateNodeHandle(),getName());
+ this->node=new IrFootSensorDriverNode(this->nodelet_nh);
// initialize the thread
this->thread_server=CThreadServer::instance();
this->spin_thread_id=getName() + "_firewire_nodelet_spin";
--
GitLab