From f3f29c98dea0b3b4c2670ab419e3b5ecd7807cf4 Mon Sep 17 00:00:00 2001 From: vcoelen Date: Wed, 23 Sep 2020 17:42:21 +0200 Subject: [PATCH 1/3] [robotino_node] Remove cablePullChangedEvent no longer in API2, Add a new service to activate compressors on the robotinoXT. [robotno_msgs] Add SetCompressorsEnabled.srv service file --- robotino_msgs/CMakeLists.txt | 1 + robotino_msgs/srv/SetCompressorsEnabled.srv | 2 + robotino_node/include/CompactBHAROS.h | 19 ++++++++- robotino_node/src/CompactBHAROS.cpp | 46 +++++++++++++++++---- 4 files changed, 57 insertions(+), 11 deletions(-) create mode 100644 robotino_msgs/srv/SetCompressorsEnabled.srv diff --git a/robotino_msgs/CMakeLists.txt b/robotino_msgs/CMakeLists.txt index 5543a3a..7e09017 100644 --- a/robotino_msgs/CMakeLists.txt +++ b/robotino_msgs/CMakeLists.txt @@ -65,6 +65,7 @@ add_message_files( add_service_files( FILES ResetOdometry.srv + SetCompressorsEnabled.srv SetEncoderPosition.srv SetGripperState.srv Grip.srv diff --git a/robotino_msgs/srv/SetCompressorsEnabled.srv b/robotino_msgs/srv/SetCompressorsEnabled.srv new file mode 100644 index 0000000..d0f476b --- /dev/null +++ b/robotino_msgs/srv/SetCompressorsEnabled.srv @@ -0,0 +1,2 @@ +bool enable +--- diff --git a/robotino_node/include/CompactBHAROS.h b/robotino_node/include/CompactBHAROS.h index cfa4d3e..4907670 100644 --- a/robotino_node/include/CompactBHAROS.h +++ b/robotino_node/include/CompactBHAROS.h @@ -13,6 +13,7 @@ #include #include "robotino_msgs/BHAReadings.h" #include "robotino_msgs/SetBHAPressures.h" +#include "robotino_msgs/SetCompressorsEnabled.h" class CompactBHAROS : public rec::robotino::api2::CompactBHA { @@ -29,14 +30,28 @@ class CompactBHAROS : public rec::robotino::api2::CompactBHA ros::Publisher bha_pub_; - robotino_msgs::BHAReadings bha_msg_; + ros::ServiceServer set_compressor_enabled_server_; + + /* TODO(vcoelen) : add these functionnalities (cf CompactBHA.h in API2 source) + setWaterDrainValve + setGripperValve1 + setGripperValve2 + */ + + robotino_msgs::BHAReadings bha_msg_; ros::Time stamp_; void pressuresChangedEvent( const float* pressures, unsigned int size ); - void cablepullChangedEvent( const float* cablepull, unsigned int size ); + void pressureSensorChangedEvent( bool pressureSensor ); + void stringPotsChangedEvent( const float* readings, unsigned int size ); + void foilPotChangedEvent( float value ); void setBHAPressuresCallback( const robotino_msgs::SetBHAPressuresConstPtr &msg); + + bool setCompressorsEnabledService( + robotino_msgs::SetCompressorsEnabled::Request &req, + robotino_msgs::SetCompressorsEnabled::Response &res); }; #endif /* COMPACTBHAROS_H_ */ diff --git a/robotino_node/src/CompactBHAROS.cpp b/robotino_node/src/CompactBHAROS.cpp index 61fa6b6..246a886 100644 --- a/robotino_node/src/CompactBHAROS.cpp +++ b/robotino_node/src/CompactBHAROS.cpp @@ -11,6 +11,8 @@ CompactBHAROS::CompactBHAROS() { bha_pub_ = nh_.advertise("bha_readings", 1, true); bha_sub_ = nh_.subscribe("set_bha_pressures", 1, &CompactBHAROS::setBHAPressuresCallback, this); + set_compressor_enabled_server_ = nh_.advertiseService("set_compressor_enabled", + &CompactBHAROS::setCompressorsEnabledService, this); } CompactBHAROS::~CompactBHAROS() @@ -35,19 +37,34 @@ void CompactBHAROS::pressuresChangedEvent( const float* pressures, unsigned int } } -void CompactBHAROS::cablepullChangedEvent( const float* cablepull, unsigned int size ) +// void CompactBHAROS::cablepullChangedEvent( const float* cablepull, unsigned int size ) +// { +// // Build the BHAReadings msg +// bha_msg_.cablepull.resize( size, 0.0 ); +// if( cablepull != NULL ) +// { +// memcpy( bha_msg_.cablepull.data(), cablepull, size * sizeof(float) ); +// } +// +// // Publish the msg +// bha_pub_.publish( bha_msg_ ); +// } +void CompactBHAROS::pressureSensorChangedEvent( bool pressureSensor ) { - // Build the BHAReadings msg - bha_msg_.cablepull.resize( size, 0.0 ); - if( cablepull != NULL ) - { - memcpy( bha_msg_.cablepull.data(), cablepull, size * sizeof(float) ); - } - // Publish the msg - bha_pub_.publish( bha_msg_ ); } +void CompactBHAROS::stringPotsChangedEvent( const float* readings, unsigned int size ) +{ + +} + +void CompactBHAROS::foilPotChangedEvent( float value ) +{ + +} + + void CompactBHAROS::setBHAPressuresCallback(const robotino_msgs::SetBHAPressuresConstPtr &msg) { float pressures[8]; @@ -62,3 +79,14 @@ void CompactBHAROS::setBHAPressuresCallback(const robotino_msgs::SetBHAPressures setPressures( pressures ); } } + + + + +bool CompactBHAROS::setCompressorsEnabledService( + robotino_msgs::SetCompressorsEnabled::Request &req, + robotino_msgs::SetCompressorsEnabled::Response &res) +{ + setCompressorsEnabled( req.enable ); + return true; +} From fa9b4fc845cb8738a67489bed1d428b194246cd5 Mon Sep 17 00:00:00 2001 From: vcoelen Date: Wed, 23 Sep 2020 17:53:56 +0200 Subject: [PATCH 2/3] [robotino_node] Ajout des Events : pressureSensor, stringPots, foilPot [robotino_msgs] Modification du message BHA_reading correspondant --- robotino_msgs/msg/BHAReadings.msg | 4 +++- robotino_node/src/CompactBHAROS.cpp | 24 ++++++++++-------------- 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/robotino_msgs/msg/BHAReadings.msg b/robotino_msgs/msg/BHAReadings.msg index 703dc18..dd14be6 100644 --- a/robotino_msgs/msg/BHAReadings.msg +++ b/robotino_msgs/msg/BHAReadings.msg @@ -1,3 +1,5 @@ time stamp float32[] pressures # in bar -float32[] cablepull \ No newline at end of file +bool pressureSensor +float32[] stringPots +float32 foilPot diff --git a/robotino_node/src/CompactBHAROS.cpp b/robotino_node/src/CompactBHAROS.cpp index 246a886..377707a 100644 --- a/robotino_node/src/CompactBHAROS.cpp +++ b/robotino_node/src/CompactBHAROS.cpp @@ -37,31 +37,27 @@ void CompactBHAROS::pressuresChangedEvent( const float* pressures, unsigned int } } -// void CompactBHAROS::cablepullChangedEvent( const float* cablepull, unsigned int size ) -// { -// // Build the BHAReadings msg -// bha_msg_.cablepull.resize( size, 0.0 ); -// if( cablepull != NULL ) -// { -// memcpy( bha_msg_.cablepull.data(), cablepull, size * sizeof(float) ); -// } -// -// // Publish the msg -// bha_pub_.publish( bha_msg_ ); -// } void CompactBHAROS::pressureSensorChangedEvent( bool pressureSensor ) { - + bha_msg_.pressureSensor = pressureSensor; } void CompactBHAROS::stringPotsChangedEvent( const float* readings, unsigned int size ) { + // Build the BHAReadings msg + bha_msg_.stringPots.resize( size, 0.0 ); + if( readings != NULL ) + { + memcpy( bha_msg_.stringPots.data(), readings, size * sizeof(float) ); + } + // Publish the msg + bha_pub_.publish( bha_msg_ ); } void CompactBHAROS::foilPotChangedEvent( float value ) { - + bha_msg_.foilPot = value; } From e92ebe10c2b33bf2588a02bd632bf7f858482a06 Mon Sep 17 00:00:00 2001 From: vcoelen Date: Wed, 23 Sep 2020 18:06:09 +0200 Subject: [PATCH 3/3] =?UTF-8?q?[robotino=5Fnode]=20Mise=20en=20conformit?= =?UTF-8?q?=C3=A9=20du=20code=20(tabulations)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- robotino_node/include/CompactBHAROS.h | 22 +++++++++++----------- robotino_node/src/CompactBHAROS.cpp | 14 +++++++------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/robotino_node/include/CompactBHAROS.h b/robotino_node/include/CompactBHAROS.h index 4907670..d14ed8d 100644 --- a/robotino_node/include/CompactBHAROS.h +++ b/robotino_node/include/CompactBHAROS.h @@ -32,26 +32,26 @@ class CompactBHAROS : public rec::robotino::api2::CompactBHA ros::ServiceServer set_compressor_enabled_server_; - /* TODO(vcoelen) : add these functionnalities (cf CompactBHA.h in API2 source) - setWaterDrainValve - setGripperValve1 - setGripperValve2 - */ + /* TODO(vcoelen) : add these functionnalities (cf CompactBHA.h in API2 source) + setWaterDrainValve + setGripperValve1 + setGripperValve2 + */ robotino_msgs::BHAReadings bha_msg_; ros::Time stamp_; void pressuresChangedEvent( const float* pressures, unsigned int size ); - void pressureSensorChangedEvent( bool pressureSensor ); - void stringPotsChangedEvent( const float* readings, unsigned int size ); - void foilPotChangedEvent( float value ); + void pressureSensorChangedEvent( bool pressureSensor ); + void stringPotsChangedEvent( const float* readings, unsigned int size ); + void foilPotChangedEvent( float value ); void setBHAPressuresCallback( const robotino_msgs::SetBHAPressuresConstPtr &msg); - bool setCompressorsEnabledService( - robotino_msgs::SetCompressorsEnabled::Request &req, - robotino_msgs::SetCompressorsEnabled::Response &res); + bool setCompressorsEnabledService( + robotino_msgs::SetCompressorsEnabled::Request &req, + robotino_msgs::SetCompressorsEnabled::Response &res); }; #endif /* COMPACTBHAROS_H_ */ diff --git a/robotino_node/src/CompactBHAROS.cpp b/robotino_node/src/CompactBHAROS.cpp index 377707a..228f402 100644 --- a/robotino_node/src/CompactBHAROS.cpp +++ b/robotino_node/src/CompactBHAROS.cpp @@ -11,7 +11,7 @@ CompactBHAROS::CompactBHAROS() { bha_pub_ = nh_.advertise("bha_readings", 1, true); bha_sub_ = nh_.subscribe("set_bha_pressures", 1, &CompactBHAROS::setBHAPressuresCallback, this); - set_compressor_enabled_server_ = nh_.advertiseService("set_compressor_enabled", + set_compressor_enabled_server_ = nh_.advertiseService("set_compressor_enabled", &CompactBHAROS::setCompressorsEnabledService, this); } @@ -39,7 +39,7 @@ void CompactBHAROS::pressuresChangedEvent( const float* pressures, unsigned int void CompactBHAROS::pressureSensorChangedEvent( bool pressureSensor ) { - bha_msg_.pressureSensor = pressureSensor; + bha_msg_.pressureSensor = pressureSensor; } void CompactBHAROS::stringPotsChangedEvent( const float* readings, unsigned int size ) @@ -57,7 +57,7 @@ void CompactBHAROS::stringPotsChangedEvent( const float* readings, unsigned int void CompactBHAROS::foilPotChangedEvent( float value ) { - bha_msg_.foilPot = value; + bha_msg_.foilPot = value; } @@ -80,9 +80,9 @@ void CompactBHAROS::setBHAPressuresCallback(const robotino_msgs::SetBHAPressures bool CompactBHAROS::setCompressorsEnabledService( - robotino_msgs::SetCompressorsEnabled::Request &req, - robotino_msgs::SetCompressorsEnabled::Response &res) + robotino_msgs::SetCompressorsEnabled::Request &req, + robotino_msgs::SetCompressorsEnabled::Response &res) { - setCompressorsEnabled( req.enable ); - return true; + setCompressorsEnabled( req.enable ); + return true; }