Skip to content

Work/vincent/bh acompatibility #6

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
vcoelen wants to merge 3 commits into
base: devel
Choose a base branch
from
Open

Work/vincent/bh acompatibility #6

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f3f29c9
[robotino_node] Remove cablePullChangedEvent no longer in API2, Add a…
vcoelen Sep 23, 2020
fa9b4fc
[robotino_node] Ajout des Events : pressureSensor, stringPots, foilPot
vcoelen Sep 23, 2020
e92ebe1
[robotino_node] Mise en conformité du code (tabulations)
vcoelen Sep 23, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions robotino_msgs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,6 +65,7 @@ add_message_files(
add_service_files(
FILES
ResetOdometry.srv
SetCompressorsEnabled.srv
SetEncoderPosition.srv
SetGripperState.srv
Grip.srv
Expand Down
4 changes: 3 additions & 1 deletion robotino_msgs/msg/BHAReadings.msg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
time stamp
float32[] pressures # in bar
float32[] cablepull
bool pressureSensor
float32[] stringPots
float32 foilPot
2 changes: 2 additions & 0 deletions robotino_msgs/srv/SetCompressorsEnabled.srv
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
bool enable
---
19 changes: 17 additions & 2 deletions robotino_node/include/CompactBHAROS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
#include <ros/ros.h>
#include "robotino_msgs/BHAReadings.h"
#include "robotino_msgs/SetBHAPressures.h"
#include "robotino_msgs/SetCompressorsEnabled.h"

class CompactBHAROS : public rec::robotino::api2::CompactBHA
{
Expand All @@ -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_ */
32 changes: 28 additions & 4 deletions robotino_node/src/CompactBHAROS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,8 @@ CompactBHAROS::CompactBHAROS()
{
bha_pub_ = nh_.advertise<robotino_msgs::BHAReadings>("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()
Expand All @@ -35,19 +37,30 @@ void CompactBHAROS::pressuresChangedEvent( const float* pressures, unsigned int
}
}

void CompactBHAROS::cablepullChangedEvent( const float* cablepull, unsigned int size )
void CompactBHAROS::pressureSensorChangedEvent( bool pressureSensor )
{
bha_msg_.pressureSensor = pressureSensor;
}

void CompactBHAROS::stringPotsChangedEvent( const float* readings, unsigned int size )
{
// Build the BHAReadings msg
bha_msg_.cablepull.resize( size, 0.0 );
if( cablepull != NULL )
bha_msg_.stringPots.resize( size, 0.0 );
if( readings != NULL )
{
memcpy( bha_msg_.cablepull.data(), cablepull, size * sizeof(float) );
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;
}


void CompactBHAROS::setBHAPressuresCallback(const robotino_msgs::SetBHAPressuresConstPtr &msg)
{
float pressures[8];
Expand All @@ -62,3 +75,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;
}