diff --git a/robotino_msgs/CMakeLists.txt b/robotino_msgs/CMakeLists.txt index 5543a3a..fae8d89 100644 --- a/robotino_msgs/CMakeLists.txt +++ b/robotino_msgs/CMakeLists.txt @@ -60,6 +60,7 @@ add_message_files( GripperState.msg PowerReadings.msg SetGrapplerAxis.msg + GyroReadings.msg ) ## Generate services in the 'srv' folder add_service_files( diff --git a/robotino_msgs/msg/GyroReadings.msg b/robotino_msgs/msg/GyroReadings.msg new file mode 100644 index 0000000..c38e213 --- /dev/null +++ b/robotino_msgs/msg/GyroReadings.msg @@ -0,0 +1,3 @@ +Header header +float32 angle +float32 rate diff --git a/robotino_msgs/msg/PowerReadings.msg b/robotino_msgs/msg/PowerReadings.msg index 2daf296..9887e98 100644 --- a/robotino_msgs/msg/PowerReadings.msg +++ b/robotino_msgs/msg/PowerReadings.msg @@ -1,3 +1,11 @@ time stamp # time-stamp -float32 current # current in A -float32 voltage # voltage in V +#float32 current # current in A +#float32 voltage # voltage in V +float32 battery_voltage +float32 system_current +bool ext_power +int32 num_chargers +#const char* batteryType +bool batteryLow +int32 batteryLowShutdownCounter + diff --git a/robotino_node/CMakeLists.txt b/robotino_node/CMakeLists.txt index 1949e8f..4c763c4 100644 --- a/robotino_node/CMakeLists.txt +++ b/robotino_node/CMakeLists.txt @@ -128,6 +128,7 @@ add_executable( src/OdometryROS.cpp src/OmniDriveROS.cpp src/PowerManagementROS.cpp + src/GyroExtROS.cpp src/RobotinoNode.cpp ) diff --git a/robotino_node/include/GyroExtROS.h b/robotino_node/include/GyroExtROS.h new file mode 100644 index 0000000..040dc4e --- /dev/null +++ b/robotino_node/include/GyroExtROS.h @@ -0,0 +1,40 @@ +/** + * \file GyroExtROS.h + * + * \brief + * + * \author Coelen Vincent (vincent.coelen@polytech-lille.net) + * \date 2017-05-31 + * \copyright 2017, Association de Robotique de Polytech Lille All rights reserved + * \version + */ + +#ifndef GYROEXTROS_H_ +#define GYROEXTROS_H_ + +#include "rec/robotino/api2/GyroscopeExt.h" + +#include +#include "robotino_msgs/GyroReadings.h" + +class GyroExtROS: public rec::robotino::api2::GyroscopeExt +{ +public: + GyroExtROS(); + ~GyroExtROS(); + + void setTimeStamp(ros::Time stamp); + +private: + ros::NodeHandle nh_; + + ros::Publisher gyro_pub_; + + robotino_msgs::GyroReadings gyro_msg_; + + ros::Time stamp_; + + void gyroscopeExtEvent(float angle, float rate); +}; + +#endif /* GYROEXTROS_H_ */ diff --git a/robotino_node/include/PowerManagementROS.h b/robotino_node/include/PowerManagementROS.h index b5db5d6..b0d64e7 100644 --- a/robotino_node/include/PowerManagementROS.h +++ b/robotino_node/include/PowerManagementROS.h @@ -29,6 +29,7 @@ class PowerManagementROS: public rec::robotino::api2::PowerManagement ros::Time stamp_; - void readingsEvent(float current, float voltage); +// void readingsEvent(float current, float voltage); + void readingsEvent( float battery_voltage, float system_current, bool ext_power, int num_chargers, const char* batteryType, bool batteryLow, int batteryLowShutdownCounter ); }; #endif /* POWERMANAGEMENTROS_H_ */ diff --git a/robotino_node/include/RobotinoNode.h b/robotino_node/include/RobotinoNode.h index 43304e1..e0a24a3 100644 --- a/robotino_node/include/RobotinoNode.h +++ b/robotino_node/include/RobotinoNode.h @@ -22,6 +22,7 @@ #include "NorthStarROS.h" #include "OmniDriveROS.h" #include "PowerManagementROS.h" +#include "GyroExtROS.h" #include #include @@ -64,6 +65,7 @@ class RobotinoNode //NorthStarROS north_star_; OmniDriveROS omni_drive_; PowerManagementROS power_management_; + GyroExtROS gyro_; void initModules(); void initMsgs(); diff --git a/robotino_node/src/GyroExtROS.cpp b/robotino_node/src/GyroExtROS.cpp new file mode 100644 index 0000000..f359fc6 --- /dev/null +++ b/robotino_node/src/GyroExtROS.cpp @@ -0,0 +1,39 @@ +/** + * \file GyroExtROS.cpp + * + * \brief + * + * \author Coelen Vincent (vincent.coelen@polytech-lille.net) + * \date 2017-05-31 + * \copyright 2017, Association de Robotique de Polytech Lille All rights reserved + * \version + */ + +#include "GyroExtROS.h" + +GyroExtROS::GyroExtROS():rec::robotino::api2::GyroscopeExt() +{ + gyro_pub_ = nh_.advertise("gyro", 1, false); +} + +GyroExtROS::~GyroExtROS() +{ + gyro_pub_.shutdown(); +} + +void GyroExtROS::setTimeStamp(ros::Time stamp) +{ + stamp_ = stamp; +} + +void GyroExtROS::gyroscopeExtEvent(float angle, float rate) +{ + // Build the Gyro message + gyro_msg_.header.stamp = stamp_; + gyro_msg_.header.frame_id = "base_link"; + + gyro_msg_.angle = angle; + gyro_msg_.rate = rate; + + gyro_pub_.publish(gyro_msg_); +} diff --git a/robotino_node/src/PowerManagementROS.cpp b/robotino_node/src/PowerManagementROS.cpp index 24e9f7c..0f19392 100644 --- a/robotino_node/src/PowerManagementROS.cpp +++ b/robotino_node/src/PowerManagementROS.cpp @@ -22,12 +22,20 @@ void PowerManagementROS::setTimeStamp(ros::Time stamp) stamp_ = stamp; } -void PowerManagementROS::readingsEvent(float current, float voltage) +//void PowerManagementROS::readingsEvent(float current, float voltage) +void PowerManagementROS::readingsEvent( float battery_voltage, float system_current, bool ext_power, int num_chargers, const char* batteryType, bool batteryLow, int batteryLowShutdownCounter ) { // Build the PowerReadings msg power_msg_.stamp = ros::Time::now(); - power_msg_.current = current; - power_msg_.voltage = voltage; + //power_msg_.current = current; + //power_msg_.voltage = voltage; + + power_msg_.battery_voltage = battery_voltage; + power_msg_.system_current = system_current; + power_msg_.ext_power = ext_power; + power_msg_.num_chargers = num_chargers; + power_msg_.batteryLow = batteryLow; + power_msg_.batteryLowShutdownCounter = batteryLowShutdownCounter; // Publish the msg power_pub_.publish( power_msg_ ); diff --git a/robotino_node/src/RobotinoNode.cpp b/robotino_node/src/RobotinoNode.cpp index cbc6f7a..ed64f81 100644 --- a/robotino_node/src/RobotinoNode.cpp +++ b/robotino_node/src/RobotinoNode.cpp @@ -49,6 +49,7 @@ void RobotinoNode::initModules() //north_star_.setComId( com_.id() ); omni_drive_.setComId( com_.id() ); power_management_.setComId( com_.id() ); + gyro_.setComId(com_.id()); omni_drive_.setMaxMin(max_linear_vel_, min_linear_vel_, max_angular_vel_, min_angular_vel_ ); com_.connectToServer( false ); } @@ -122,6 +123,7 @@ bool RobotinoNode::spin() motor_array_.setTimeStamp(curr_time_); //north_star_.setTimeStamp(curr_time_); power_management_.setTimeStamp(curr_time_); + gyro_.setTimeStamp(curr_time_); publishDistanceMsg(); publishJointStateMsg(); @@ -131,4 +133,3 @@ bool RobotinoNode::spin() } return true; } -