From c800e1236b5f9b72b3a08e1f4a1e033e2faf5703 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Tue, 29 Aug 2017 19:07:04 +0200
Subject: [PATCH] Added a nodelet for the IR foot sensor node.
---
ir_foot_sensor/CMakeLists.txt | 12 ++-
.../include/ir_foot_sensor_driver.h | 2 +-
.../include/ir_foot_sensor_driver_node.h | 25 ++++++
.../ir_foot_sensor_nodelet_plugin.xml | 7 ++
ir_foot_sensor/launch/darwin_left_foot.launch | 13 +++
.../launch/darwin_right_foot.launch | 13 +++
.../launch/darwin_two_feet_nodelet.launch | 36 +++++++++
ir_foot_sensor/package.xml | 7 +-
ir_foot_sensor/src/ir_foot_sensor_driver.cpp | 4 +-
.../src/ir_foot_sensor_driver_node.cpp | 79 +++++++++++++++++--
10 files changed, 182 insertions(+), 16 deletions(-)
create mode 100755 ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
create mode 100644 ir_foot_sensor/launch/darwin_left_foot.launch
create mode 100644 ir_foot_sensor/launch/darwin_right_foot.launch
create mode 100755 ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
diff --git a/ir_foot_sensor/CMakeLists.txt b/ir_foot_sensor/CMakeLists.txt
index 63f507a..ca5fdd2 100644
--- a/ir_foot_sensor/CMakeLists.txt
+++ b/ir_foot_sensor/CMakeLists.txt
@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
# ********************************************************************
# Add catkin additional components here
# ********************************************************************
-find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs)
+find_package(catkin REQUIRED COMPONENTS iri_base_driver humanoid_common_msgs nodelet)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
@@ -53,7 +53,7 @@ find_package(ir_feet REQUIRED)
# ********************************************************************
# Add the dynamic reconfigure file
# ********************************************************************
-generate_dynamic_reconfigure_options(cfg/IrFeetSensor.cfg)
+generate_dynamic_reconfigure_options(cfg/IrFootSensor.cfg)
# ********************************************************************
# Add run time dependencies here
@@ -64,7 +64,7 @@ catkin_package(
# ********************************************************************
# Add ROS and IRI ROS run time dependencies
# ********************************************************************
- CATKIN_DEPENDS iri_base_driver humanoid_common_msgs
+ CATKIN_DEPENDS iri_base_driver humanoid_common_msgs nodelet
# ********************************************************************
# Add system and labrobotica run time dependencies here
# ********************************************************************
@@ -89,6 +89,7 @@ include_directories(${ir_feet_INCLUDE_DIR})
## Declare a cpp library
# add_library(${PROJECT_NAME} <list of source files>)
# add_library(${PROJECT_NAME} <list of source files>)
+add_library(${PROJECT_NAME}_nodelet src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
## Declare a cpp executable
add_executable(${PROJECT_NAME} src/ir_foot_sensor_driver.cpp src/ir_foot_sensor_driver_node.cpp)
@@ -103,6 +104,11 @@ target_link_libraries(${PROJECT_NAME} ${dynamixel_LIBRARY})
target_link_libraries(${PROJECT_NAME} ${ir_feet_LIBRARY})
# target_link_libraries(${PROJECT_NAME} ${<dependency>_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${catkin_LIBRARIES})
+target_link_libraries(${PROJECT_NAME}_nodelet ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${comm_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${dynamixel_LIBRARY})
+target_link_libraries(${PROJECT_NAME}_nodelet ${ir_feet_LIBRARY})
# ********************************************************************
# Add message headers dependencies
# ********************************************************************
diff --git a/ir_foot_sensor/include/ir_foot_sensor_driver.h b/ir_foot_sensor/include/ir_foot_sensor_driver.h
index 60c36c7..abed4b7 100644
--- a/ir_foot_sensor/include/ir_foot_sensor_driver.h
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver.h
@@ -55,7 +55,7 @@ class IrFootSensorDriver : public iri_base_driver::IriBaseDriver
{
private:
// private attributes and methods
- CIRFoot<CDynamixelServerSerial> *device;
+ CIRFeet<CDynamixelServerSerial> *device;
public:
/**
* \brief define config type
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 186a6ef..b93c024 100644
--- a/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
+++ b/ir_foot_sensor/include/ir_foot_sensor_driver_node.h
@@ -182,4 +182,29 @@ class IrFootSensorDriverNode : public iri_base_driver::IriBaseNodeDriver<IrFootS
};
+#include "nodelet/nodelet.h"
+
+class IrFootSensorDriverNodelet : public nodelet::Nodelet
+{
+ private:
+ IrFootSensorDriverNode *node;
+ virtual void onInit();// initialization function
+ // thread attributes
+ CThreadServer *thread_server;
+ std::string spin_thread_id;
+ protected:
+ static void *spin_thread(void *param);
+ public:
+ IrFootSensorDriverNodelet();
+
+ /**
+ * \brief Destructor
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~IrFootSensorDriverNodelet();
+};
+
+
#endif
diff --git a/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml b/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
new file mode 100755
index 0000000..b8c9d61
--- /dev/null
+++ b/ir_foot_sensor/ir_foot_sensor_nodelet_plugin.xml
@@ -0,0 +1,7 @@
+<library path="lib/libir_foot_sensor_nodelet">
+ <class name="ir_foot_sensor/IrFootSensorDriverNodelet" type="IrFootSensorDriverNodelet" base_class_type="nodelet::Nodelet">
+ <description>
+ Nodelete for the IR foot sensor driver ROS wrapper
+ </description>
+ </class>
+</library>
diff --git a/ir_foot_sensor/launch/darwin_left_foot.launch b/ir_foot_sensor/launch/darwin_left_foot.launch
new file mode 100644
index 0000000..61d8490
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_left_foot.launch
@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+ <node name="darwin_left_foot"
+ pkg="ir_foot_sensor"
+ type="ir_foot_sensor"
+ output="screen">
+ <param name="dyn_serial" value="A4008atn"/>
+ <param name="dyn_baudrate" value="1000000"/>
+ <param name="ir_foot_id" value="1"/>
+ <remap from="/darwin_left_foot/sensor_data"
+ to="/darwin/sensors/left_foot_data"/>
+ </node>
+</launch>
diff --git a/ir_foot_sensor/launch/darwin_right_foot.launch b/ir_foot_sensor/launch/darwin_right_foot.launch
new file mode 100644
index 0000000..df674f2
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_right_foot.launch
@@ -0,0 +1,13 @@
+<!-- -->
+<launch>
+ <node name="darwin_right_foot"
+ pkg="ir_foot_sensor"
+ type="ir_foot_sensor"
+ output="screen">
+ <param name="dyn_serial" value="A4008atn"/>
+ <param name="dyn_baudrate" value="1000000"/>
+ <param name="ir_foot_id" value="2"/>
+ <remap from="/darwin_right_foot/sensor_data"
+ to="/darwin/sensors/right_foot_data"/>
+ </node>
+</launch>
diff --git a/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
new file mode 100755
index 0000000..210fb48
--- /dev/null
+++ b/ir_foot_sensor/launch/darwin_two_feet_nodelet.launch
@@ -0,0 +1,36 @@
+<!-- -->
+<launch>
+ <node pkg="nodelet"
+ type="nodelet"
+ name="dynamixel_devices"
+ args="manager"
+ output="screen">
+ </node>
+
+ <!-- left foot -->
+ <node pkg="nodelet"
+ type="nodelet"
+ 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="ir_foot_id" value="1"/>
+ <remap from="/darwin_left_foot/sensor_data"
+ to="/darwin/sensors/left_foot_data"/>
+ </node>
+
+ <!-- rught foot -->
+ <node pkg="nodelet"
+ type="nodelet"
+ 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="ir_foot_id" value="2"/>
+ <remap from="/darwin_right_foot/sensor_data"
+ to="/darwin/sensors/right_foot_data"/>
+ </node>
+
+</launch>
diff --git a/ir_foot_sensor/package.xml b/ir_foot_sensor/package.xml
index dd9ede5..b384adb 100644
--- a/ir_foot_sensor/package.xml
+++ b/ir_foot_sensor/package.xml
@@ -42,13 +42,14 @@
<buildtool_depend>catkin</buildtool_depend>
<build_depend>iri_base_driver</build_depend>
<build_depend>humanoid_common_msgs</build_depend>
+ <build_depend>nodelet</build_depend>
<run_depend>iri_base_driver</run_depend>
<run_depend>humanoid_common_msgs</run_depend>
+ <run_depend>nodelet</run_depend>
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
-
+ <nodelet plugin="${prefix}/ir_foot_sensor_nodelet_plugin.xml" />
</export>
+
</package>
diff --git a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
index a5ab37b..7d71cff 100644
--- a/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver.cpp
@@ -16,8 +16,8 @@ bool IrFootSensorDriver::openDriver(void)
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);
+ 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);
return true;
}catch(CException &e){
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 adebbbb..a3773e6 100644
--- a/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
+++ b/ir_foot_sensor/src/ir_foot_sensor_driver_node.cpp
@@ -4,7 +4,7 @@ IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
iri_base_driver::IriBaseNodeDriver<IrFootSensorDriver>(nh)
{
//init class attributes if necessary
- //this->loop_rate_ = 2;//in [Hz]
+ 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);
@@ -18,28 +18,49 @@ IrFootSensorDriverNode::IrFootSensorDriverNode(ros::NodeHandle &nh) :
// [init action servers]
// [init action clients]
+ this->sensor_data_ir_foot_data_msg_.names.resize(10);
+ this->sensor_data_ir_foot_data_msg_.names[0]="down_left_middle";
+ this->sensor_data_ir_foot_data_msg_.names[1]="down_left_rear";
+ this->sensor_data_ir_foot_data_msg_.names[2]="down_analog";
+ this->sensor_data_ir_foot_data_msg_.names[3]="down_left_front";
+ this->sensor_data_ir_foot_data_msg_.names[4]="down_right_rear";
+ this->sensor_data_ir_foot_data_msg_.names[5]="down_right_middle";
+ this->sensor_data_ir_foot_data_msg_.names[6]="down_right_front";
+ 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";
}
void IrFootSensorDriverNode::mainNodeThread(void)
{
+ std::vector<bool> status;
//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;
-
+ if(this->driver_.isRunning())
+ {
+ this->sensor_data_ir_foot_data_msg_.header.stamp=ros::Time::now();
+ this->sensor_data_ir_foot_data_msg_.voltages=this->driver_.get_all_sensor_voltages();
+ status=this->driver_.get_all_sensor_status();
+ this->sensor_data_ir_foot_data_msg_.status.resize(status.size());
+ for(unsigned int i=0;i<status.size();i++)
+ {
+ if(status[i])
+ this->sensor_data_ir_foot_data_msg_.status[i]=0x01;
+ else
+ this->sensor_data_ir_foot_data_msg_.status[i]=0x00;
+ }
+ this->sensor_data_publisher_.publish(this->sensor_data_ir_foot_data_msg_);
+ }
// [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();
@@ -87,3 +108,47 @@ int main(int argc,char *argv[])
{
return driver_base::main<IrFootSensorDriverNode>(argc, argv, "ir_foot_sensor_driver_node");
}
+
+#include <pluginlib/class_list_macros.h>
+
+IrFootSensorDriverNodelet::IrFootSensorDriverNodelet()
+{
+ this->node=NULL;
+}
+
+IrFootSensorDriverNodelet::~IrFootSensorDriverNodelet(void)
+{
+ // kill the thread
+ this->thread_server->kill_thread(this->spin_thread_id);
+ this->thread_server->detach_thread(this->spin_thread_id);
+ this->thread_server->delete_thread(this->spin_thread_id);
+ this->spin_thread_id="";
+ // [free dynamic memory]
+ if(this->node!=NULL)
+ delete this->node;
+}
+
+void IrFootSensorDriverNodelet::onInit()
+{
+ // initialize the driver node
+ this->node=new IrFootSensorDriverNode(getPrivateNodeHandle());
+ // initialize the thread
+ this->thread_server=CThreadServer::instance();
+ this->spin_thread_id=getName() + "_firewire_nodelet_spin";
+ this->thread_server->create_thread(this->spin_thread_id);
+ this->thread_server->attach_thread(this->spin_thread_id,this->spin_thread,this);
+ // start the spin thread
+ this->thread_server->start_thread(this->spin_thread_id);
+}
+
+void *IrFootSensorDriverNodelet::spin_thread(void *param)
+{
+ IrFootSensorDriverNodelet *nodelet=(IrFootSensorDriverNodelet *)param;
+
+ nodelet->node->spin();
+
+ pthread_exit(NULL);
+}
+
+// parameters are: package, class name, class type, base class type
+PLUGINLIB_DECLARE_CLASS(ir_foot_sensor, IrFootSensorDriverNodelet, IrFootSensorDriverNodelet, nodelet::Nodelet);
--
GitLab