diff --git a/ir_feet_sensor/CMakeLists.txt b/ir_feet_sensor/CMakeLists.txt
index 19c6fd189fed84df6790023e0b04e5b211cdba43..63f507aa83f7b60a1326ac88d037e6c4f39cd6b3 100644
--- a/ir_feet_sensor/CMakeLists.txt
+++ b/ir_feet_sensor/CMakeLists.txt
@@ -1,12 +1,12 @@
cmake_minimum_required(VERSION 2.8.3)
-project(ir_feet_sensor)
+project(ir_foot_sensor)
## Find catkin macros and libraries
find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -17,7 +17,7 @@ find_package(catkin REQUIRED COMPONENTS iri_base_driver)
# find_package(<dependency> REQUIRED)
find_package(iriutils REQUIRED)
find_package(comm REQUIRED)
-find_package(iriutils REQUIRED)
+find_package(dynamixel REQUIRED)
find_package(ir_feet REQUIRED)
# ********************************************************************
@@ -64,7 +64,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver
+ CATKIN_DEPENDS iri_base_driver humanoid_common_msgs
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -91,7 +91,7 @@ include_directories(${ir_feet_INCLUDE_DIR})
# add_library(${PROJECT_NAME} <list of source files>)
## Declare a cpp executable
-add_executable(${PROJECT_NAME} src/ir_feet_sensor_driver.cpp src/ir_feet_sensor_driver_node.cpp)
+add_executable(${PROJECT_NAME} src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
# ********************************************************************
# Add the libraries
@@ -107,6 +107,7 @@ target_link_libraries(${PROJECT_NAME} ${ir_feet_LIBRARY})
# Add message headers dependencies
# ********************************************************************
# add_dependencies(${PROJECT_NAME} <msg_package_name>_generate_messages_cpp)
+add_dependencies(${PROJECT_NAME} humanoid_common_msgs_generate_messages_cpp)
# ********************************************************************
# Add dynamic reconfigure dependencies
# ********************************************************************
diff --git a/ir_feet_sensor/cfg/IrFeetSensor.cfg b/ir_feet_sensor/cfg/IrFootSensor.cfg
similarity index 96%
rename from ir_feet_sensor/cfg/IrFeetSensor.cfg
rename to ir_feet_sensor/cfg/IrFootSensor.cfg
index 66d648a93784391359a5dd63dc717e88a5492cd3..6de3d38401909ee6fd251a73e56549e8a10cc8f3 100755
--- a/ir_feet_sensor/cfg/IrFeetSensor.cfg
+++ b/ir_feet_sensor/cfg/IrFootSensor.cfg
@@ -31,7 +31,7 @@
# Author:
-PACKAGE='ir_feet_sensor'
+PACKAGE='ir_foot_sensor'
from driver_base.msg import SensorLevels
from dynamic_reconfigure.parameter_generator_catkin import *
@@ -44,4 +44,4 @@ gen.add("dyn_serial", str_t, SensorLevels.RECONFIGURE_STOP,
gen.add("dyn_baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Dynamixel serial baudrate", 1000000, 9600, 3000000)
gen.add("ir_foot_id", int_t, SensorLevels.RECONFIGURE_STOP, "IR foot identifier", 1, 0, 254)
-exit(gen.generate(PACKAGE, "IrFeetSensorDriver", "IrFeetSensor"))
+exit(gen.generate(PACKAGE, "IrFootSensorDriver", "IrFootSensor"))
diff --git a/ir_feet_sensor/include/ir_feet_sensor_driver.h b/ir_feet_sensor/include/ir_foot_sensor_driver.h
similarity index 88%
rename from ir_feet_sensor/include/ir_feet_sensor_driver.h
rename to ir_feet_sensor/include/ir_foot_sensor_driver.h
index 2e546e2c2c76d86ed951a1eebeff68a151517f3a..60c36c70497a8ac1b056ea007bc4e4bfff04d738 100644
--- a/ir_feet_sensor/include/ir_feet_sensor_driver.h
+++ b/ir_feet_sensor/include/ir_foot_sensor_driver.h
@@ -22,13 +22,13 @@
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab
-#ifndef _ir_feet_sensor_driver_h_
-#define _ir_feet_sensor_driver_h_
+#ifndef _ir_foot_sensor_driver_h_
+#define _ir_foot_sensor_driver_h_
#include <iri_base_driver/iri_base_driver.h>
-#include <ir_feet_sensor/IrFeetSensorConfig.h>
+#include <ir_foot_sensor/IrFootSensorConfig.h>
-//include ir_feet_sensor_driver main library
+//include ir_foot_sensor_driver main library
#include "dynamixelserver_serial.h"
#include "ir_feet.h"
@@ -41,29 +41,29 @@
* ROS driver structure. ROS driver_base state transitions are already managed
* by IriBaseDriver.
*
- * The IrFeetSensorDriver class must implement all specific driver requirements to
+ * The IrFootSensorDriver class must implement all specific driver requirements to
* safetely open, close, run and stop the driver at any time. It also must
* guarantee an accessible interface for all driver's parameters.
*
- * The IrFeetSensorConfig.cfg needs to be filled up with those parameters suitable
+ * The IrFootSensorConfig.cfg needs to be filled up with those parameters suitable
* to be changed dynamically by the ROS dyanmic reconfigure application. The
* implementation of the CIriNode class will manage those parameters through
* methods like postNodeOpenHook() and reconfigureNodeHook().
*
*/
-class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
+class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
{
private:
// private attributes and methods
- CIRFeet<CDynamixelServerSerial> *device;
+ CIRFoot<CDynamixelServerSerial> *device;
public:
/**
* \brief define config type
*
- * Define a Config type with the IrFeetSensorConfig. All driver implementations
+ * Define a Config type with the IrFootSensorConfig. All driver implementations
* will then use the same variable type Config.
*/
- typedef ir_feet_sensor::IrFeetSensorConfig Config;
+ typedef ir_foot_sensor::IrFootSensorConfig Config;
/**
* \brief config variable
@@ -81,7 +81,7 @@ class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
* Attributes from the main node driver class IriBaseDriver such as loop_rate,
* may be also overload here.
*/
- IrFeetSensorDriver(void);
+ IrFootSensorDriver(void);
/**
* \brief open driver
@@ -144,7 +144,7 @@ class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
*/
void config_update(Config& new_cfg, uint32_t level=0);
- // here define all ir_feet_sensor_driver interface methods to retrieve and set
+ // here define all ir_foot_sensor_driver interface methods to retrieve and set
// the driver parameters
std::vector<double> get_all_sensor_voltages(void);
std::vector<double> get_all_sensor_thresholds(void);
@@ -157,7 +157,7 @@ class IrFeetSensorDriver : public iri_base_driver::IriBaseDriver
* This destructor is called when the object is about to be destroyed.
*
*/
- ~IrFeetSensorDriver(void);
+ ~IrFootSensorDriver(void);
};
#endif
diff --git a/ir_feet_sensor/include/ir_feet_sensor_driver_node.h b/ir_feet_sensor/include/ir_foot_sensor_driver_node.h
similarity index 92%
rename from ir_feet_sensor/include/ir_feet_sensor_driver_node.h
rename to ir_feet_sensor/include/ir_foot_sensor_driver_node.h
index 882899dcec04e5336d4f0c5e3854e35661b92626..186a6ef9b3bc5023343df79ae9a295bc63813935 100644
--- a/ir_feet_sensor/include/ir_feet_sensor_driver_node.h
+++ b/ir_feet_sensor/include/ir_foot_sensor_driver_node.h
@@ -22,13 +22,14 @@
// refer to the IRI wiki page for more information:
// http://wikiri.upc.es/index.php/Robotics_Lab
-#ifndef _ir_feet_sensor_driver_node_h_
-#define _ir_feet_sensor_driver_node_h_
+#ifndef _ir_foot_sensor_driver_node_h_
+#define _ir_foot_sensor_driver_node_h_
#include <iri_base_driver/iri_base_driver_node.h>
-#include "ir_feet_sensor_driver.h"
+#include "ir_foot_sensor_driver.h"
// [publisher subscriber headers]
+#include <humanoid_common_msgs/ir_foot_data.h>
// [service client headers]
@@ -51,10 +52,13 @@
* http://www.ros.org/wiki/diagnostics/ (Tutorials: Creating a Diagnostic Analyzer)
* http://www.ros.org/wiki/self_test/ (Example: Self Test)
*/
-class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>
+class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>
{
private:
// [publisher attributes]
+ ros::Publisher sensor_data_publisher_;
+ humanoid_common_msgs::ir_foot_data sensor_data_ir_foot_data_msg_;
+
// [subscriber attributes]
@@ -79,7 +83,7 @@ class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetS
/**
* \brief constructor
*
- * This constructor mainly creates and initializes the IrFeetSensorDriverNode topics
+ * This constructor mainly creates and initializes the IrFootSensorDriverNode topics
* through the given public_node_handle object. IriBaseNodeDriver attributes
* may be also modified to suit node specifications.
*
@@ -93,7 +97,7 @@ class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetS
*
* \param nh a reference to the node handle object to manage all ROS topics.
*/
- IrFeetSensorDriverNode(ros::NodeHandle& nh);
+ IrFootSensorDriverNode(ros::NodeHandle& nh);
/**
* \brief Destructor
@@ -101,7 +105,7 @@ class IrFeetSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFeetS
* This destructor is called when the object is about to be destroyed.
*
*/
- ~IrFeetSensorDriverNode(void);
+ ~IrFootSensorDriverNode(void);
protected:
/**
diff --git a/ir_feet_sensor/package.xml b/ir_feet_sensor/package.xml
index da2d8914fdd7a7093752844e07256554d581cf1b..dd9ede5c0c1a5ee0e70c8bb6a4506d89c9304275 100644
--- a/ir_feet_sensor/package.xml
+++ b/ir_feet_sensor/package.xml
@@ -1,8 +1,8 @@
<?xml version="1.0"?>
<package>
- <name>ir_feet_sensor</name>
+ <name>ir_foot_sensor</name>
<version>0.0.0</version>
- <description>The ir_feet_sensor package</description>
+ <description>The ir_foot_sensor package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
@@ -19,7 +19,7 @@
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/ir_feet_sensor</url> -->
+ <!-- <url type="website">http://wiki.ros.org/ir_foot_sensor</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
@@ -41,7 +41,9 @@
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_driver</build_depend>
+ <build_depend>humanoid_common_msgs</build_depend>
<run_depend>iri_base_driver</run_depend>
+ <run_depend>humanoid_common_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
diff --git a/ir_feet_sensor/src/ir_feet_sensor_driver.cpp b/ir_feet_sensor/src/ir_feet_sensor_driver.cpp
deleted file mode 100644
index 93ddbd654cf3675dcd326f0c5d9460925903b55d..0000000000000000000000000000000000000000
--- a/ir_feet_sensor/src/ir_feet_sensor_driver.cpp
+++ /dev/null
@@ -1,99 +0,0 @@
-#include "ir_feet_sensor_driver.h"
-
-IrFeetSensorDriver::IrFeetSensorDriver(void)
-{
- //setDriverId(driver string id);
- this->device=NULL;
-}
-
-bool IrFeetSensorDriver::openDriver(void)
-{
- try{
- if(this->device!=NULL)
- {
- delete this->device;
- this->device=NULL;
- }
- this->device=new CIRFeet<CDynamixelServerSerial>("feet",this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
-
- return true;
- }catch(CException &e){
- ROS_WARN_STREAM(e.what());
- return false;
- }
-}
-
-bool IrFeetSensorDriver::closeDriver(void)
-{
- try{
- if(this->darwin!=NULL)
- {
- delete this->darwin;
- this->darwin=NULL;
- }
- return true;
- }catch(CException &e){
- ROS_WARN_STREAM(e.what());
- return false;
- }
-}
-
-bool IrFeetSensorDriver::startDriver(void)
-{
- return true;
-}
-
-bool IrFeetSensorDriver::stopDriver(void)
-{
- return true;
-}
-
-void IrFeetSensorDriver::config_update(Config& new_cfg, uint32_t level)
-{
- this->lock();
-
- // depending on current state
- // update driver with new_cfg data
- switch(this->getState())
- {
- case IrFeetSensorDriver::CLOSED:
- break;
-
- case IrFeetSensorDriver::OPENED:
- break;
-
- case IrFeetSensorDriver::RUNNING:
- break;
- }
-
- // save the current configuration
- this->config_=new_cfg;
-
- this->unlock();
-}
-
-std::vector<double> IrFeetSensorDriver::get_all_sensor_voltages(void)
-{
-
-}
-
-std::vector<double> IrFeetSensorDriver::get_all_sensor_thresholds(void)
-{
-
-}
-
-void IrFeetSensorDriver::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
-{
-
-}
-
-std::vector<bool> IrFeetSensorDriver::get_all_sensor_status(void)
-{
-
-}
-
-IrFeetSensorDriver::~IrFeetSensorDriver(void)
-{
- if(this->device!=NULL)
- delete this->device;
-}
diff --git a/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp b/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp
deleted file mode 100644
index 0faf9d0494587993ccebca032dcf03efd6c1b639..0000000000000000000000000000000000000000
--- a/ir_feet_sensor/src/ir_feet_sensor_driver_node.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-#include "ir_feet_sensor_driver_node.h"
-
-IrFeetSensorDriverNode::IrFeetSensorDriverNode(ros::NodeHandle &nh) :
- iri_base_driver::IriBaseNodeDriver<IrFeetSensorDriver>(nh)
-{
- //init class attributes if necessary
- //this->loop_rate_ = 2;//in [Hz]
-
- // [init publishers]
-
- // [init subscribers]
-
- // [init services]
-
- // [init clients]
-
- // [init action servers]
-
- // [init action clients]
-}
-
-void IrFeetSensorDriverNode::mainNodeThread(void)
-{
- //lock access to driver if necessary
- this->driver_.lock();
-
- // [fill msg Header if necessary]
-
- // [fill msg structures]
-
- // [fill srv structure and make request to the server]
-
- // [fill action structure and make request to the action server]
-
- // [publish messages]
-
- //unlock access to driver if previously blocked
- this->driver_.unlock();
-}
-
-/* [subscriber callbacks] */
-
-/* [service callbacks] */
-
-/* [action callbacks] */
-
-/* [action requests] */
-
-void IrFeetSensorDriverNode::postNodeOpenHook(void)
-{
-}
-
-void IrFeetSensorDriverNode::addNodeDiagnostics(void)
-{
-}
-
-void IrFeetSensorDriverNode::addNodeOpenedTests(void)
-{
-}
-
-void IrFeetSensorDriverNode::addNodeStoppedTests(void)
-{
-}
-
-void IrFeetSensorDriverNode::addNodeRunningTests(void)
-{
-}
-
-void IrFeetSensorDriverNode::reconfigureNodeHook(int level)
-{
-}
-
-IrFeetSensorDriverNode::~IrFeetSensorDriverNode(void)
-{
- // [free dynamic memory]
-}
-
-/* main function */
-int main(int argc,char *argv[])
-{
- return driver_base::main<IrFeetSensorDriverNode>(argc, argv, "ir_feet_sensor_driver_node");
-}
diff --git a/ir_feet_sensor/src/ir_foot_sensor_driver.cpp b/ir_feet_sensor/src/ir_foot_sensor_driver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5ab37b9eb30051abf1080257da70265cbdbc578
--- /dev/null
+++ b/ir_feet_sensor/src/ir_foot_sensor_driver.cpp
@@ -0,0 +1,112 @@
+#include "ir_foot_sensor_driver.h"
+
+IrFootSensorDriver::IrFootSensorDriver(void)
+{
+ //setDriverId(driver string id);
+ this->device=NULL;
+}
+
+bool IrFootSensorDriver::openDriver(void)
+{
+ std::stringstream name;
+
+ try{
+ if(this->device!=NULL)
+ {
+ delete this->device;
+ this->device=NULL;
+ }
+ name << foot << "_" << this->config_.ir_foot_id;
+ this->device=new CIRFoot<CDynamixelServerSerial>(name.str(),this->config_.dyn_serial,this->config_.dyn_baudrate,this->config_.ir_foot_id);
+
+ return true;
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ return false;
+ }
+}
+
+bool IrFootSensorDriver::closeDriver(void)
+{
+ try{
+ if(this->device!=NULL)
+ {
+ delete this->device;
+ this->device=NULL;
+ }
+ return true;
+ }catch(CException &e){
+ ROS_WARN_STREAM(e.what());
+ return false;
+ }
+}
+
+bool IrFootSensorDriver::startDriver(void)
+{
+ return true;
+}
+
+bool IrFootSensorDriver::stopDriver(void)
+{
+ return true;
+}
+
+void IrFootSensorDriver::config_update(Config& new_cfg, uint32_t level)
+{
+ this->lock();
+
+ // depending on current state
+ // update driver with new_cfg data
+ switch(this->getState())
+ {
+ case IrFootSensorDriver::CLOSED:
+ break;
+
+ case IrFootSensorDriver::OPENED:
+ break;
+
+ case IrFootSensorDriver::RUNNING:
+ break;
+ }
+
+ // save the current configuration
+ this->config_=new_cfg;
+
+ this->unlock();
+}
+
+std::vector<double> IrFootSensorDriver::get_all_sensor_voltages(void)
+{
+ if(this->device!=NULL)
+ return this->device->get_all_sensor_voltages();
+ else
+ return std::vector<double>();
+}
+
+std::vector<double> IrFootSensorDriver::get_all_sensor_thresholds(void)
+{
+ if(this->device!=NULL)
+ return this->device->get_all_sensor_thresholds();
+ else
+ return std::vector<double>();
+}
+
+void IrFootSensorDriver::set_sensor_threshold(adc_dma_ch_t channel_id, double threshold)
+{
+ if(this->device!=NULL)
+ return this->device->set_sensor_threshold(channel_id, threshold);
+}
+
+std::vector<bool> IrFootSensorDriver::get_all_sensor_status(void)
+{
+ if(this->device!=NULL)
+ return this->device->get_all_sensor_status();
+ else
+ return std::vector<bool>();
+}
+
+IrFootSensorDriver::~IrFootSensorDriver(void)
+{
+ if(this->device!=NULL)
+ delete this->device;
+}
diff --git a/ir_feet_sensor/src/ir_foot_sensor_driver_node.cpp b/ir_feet_sensor/src/ir_foot_sensor_driver_node.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..adebbbb46598084db4ff784106a26c46bff29f71
--- /dev/null
+++ b/ir_feet_sensor/src/ir_foot_sensor_driver_node.cpp
@@ -0,0 +1,89 @@
+#include "ir_foot_sensor_driver_node.h"
+
+IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
+ iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
+{
+ //init class attributes if necessary
+ //this->loop_rate_ = 2;//in [Hz]
+
+ // [init publishers]
+ this->sensor_data_publisher_ = this->public_node_handle_.advertise<humanoid_common_msgs::ir_foot_data>("sensor_data", 1);
+
+ // [init subscribers]
+
+ // [init services]
+
+ // [init clients]
+
+ // [init action servers]
+
+ // [init action clients]
+}
+
+void IrFootSensorDriverNode::mainNodeThread(void)
+{
+ //lock access to driver if necessary
+ this->driver_.lock();
+
+ // [fill msg Header if necessary]
+
+ // [fill msg structures]
+ // Initialize the topic message structure
+ //this->sensor_data_ir_foot_data_msg_.data = my_var;
+
+
+ // [fill srv structure and make request to the server]
+
+ // [fill action structure and make request to the action server]
+
+ // [publish messages]
+ // Uncomment the following line to publish the topic message
+ //this->sensor_data_publisher_.publish(this->sensor_data_ir_foot_data_msg_);
+
+
+ //unlock access to driver if previously blocked
+ this->driver_.unlock();
+}
+
+/* [subscriber callbacks] */
+
+/* [service callbacks] */
+
+/* [action callbacks] */
+
+/* [action requests] */
+
+void IrFootSensorDriverNode::postNodeOpenHook(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeDiagnostics(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeOpenedTests(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeStoppedTests(void)
+{
+}
+
+void IrFootSensorDriverNode::addNodeRunningTests(void)
+{
+}
+
+void IrFootSensorDriverNode::reconfigureNodeHook(int level)
+{
+}
+
+IrFootSensorDriverNode::~IrFootSensorDriverNode(void)
+{
+ // [free dynamic memory]
+}
+
+/* main function */
+int main(int argc,char *argv[])
+{
+ return driver_base::main<IrFootSensorDriverNode>(argc, argv, "ir_foot_sensor_driver_node");
+}