diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver.h b/ir_foot_sensor/include/ir_foot_sensor_driver.h
index abed4b7efbe8c85749eca95cc3cd4539d1a2821e..e459ddee0253f3e45d462df023c55a7d8a89832a 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 b93c024f8c14effa535cb3a55bacebb2e00b877f..1b28e08ea87493d1020c79d692cfca752159d406 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 61d8490bbcf62b782b0ce78dd37caab56b9537c2..3e257823e9c2242345f05011ae4318b46b6d67c0 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 df674f2e27fbaca24d9a718ab36215795c18c1f7..51cd6f397c8db7ca607ad140b8157ba17edbaac0 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 210fb48de7eb1219ee26db4962c9a16ba7cf370a..7146cd77bf5178244d6878b9341a4034ad626477 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 7d71cff775c6dfa1ba453ca15cd01c7020f0737d..18fdc9b55317bea13d497e3a66c6189eb5724dfe 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 a3773e6b8dfeb53db1de6a05f76e93cce81c3f02..1dcea828e30b45a1e252714679ff696e3fe10e0a 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";