diff --git a/ir_foot_sensor/CMakeLists.txt b/ir_foot_sensor/CMakeLists.txt index 63f507aa83f7b60a1326ac88d037e6c4f39cd6b3..ca5fdd2b28c9fa5819df9bb179ec94d90aee7e57 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 60c36c70497a8ac1b056ea007bc4e4bfff04d738..abed4b7efbe8c85749eca95cc3cd4539d1a2821e 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 186a6ef9b3bc5023343df79ae9a295bc63813935..b93c024f8c14effa535cb3a55bacebb2e00b877f 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 0000000000000000000000000000000000000000..b8c9d6193ba2a82bb7400d270c2fe83c3a835711 --- /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 0000000000000000000000000000000000000000..61d8490bbcf62b782b0ce78dd37caab56b9537c2 --- /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 0000000000000000000000000000000000000000..df674f2e27fbaca24d9a718ab36215795c18c1f7 --- /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 0000000000000000000000000000000000000000..210fb48de7eb1219ee26db4962c9a16ba7cf370a --- /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 dd9ede5c0c1a5ee0e70c8bb6a4506d89c9304275..b384adb3b9bde53c329e9386a90e68ce1ac7c459 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 a5ab37b9eb30051abf1080257da70265cbdbc578..7d71cff775c6dfa1ba453ca15cd01c7020f0737d 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 adebbbb46598084db4ff784106a26c46bff29f71..a3773e6b8dfeb53db1de6a05f76e93cce81c3f02 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);