WIP : PowerReadings

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(

robotino_msgs/msg/GyroReadings.msg

@@ -0,0 +1,3 @@
+Header header
+float32 angle
+float32 rate

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
+

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
 )
 

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 <ros/ros.h>
+#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_ */

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_ */

robotino_node/include/RobotinoNode.h

@@ -22,6 +22,7 @@
 #include "NorthStarROS.h"
 #include "OmniDriveROS.h"
 #include "PowerManagementROS.h"
+#include "GyroExtROS.h"
 
 #include <ros/ros.h>
 #include <sensor_msgs/PointCloud.h>
@@ -64,6 +65,7 @@ class RobotinoNode
 	//NorthStarROS north_star_;
 	OmniDriveROS omni_drive_;
 	PowerManagementROS power_management_;
+    GyroExtROS gyro_;
 
 	void initModules();
 	void initMsgs();

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<robotino_msgs::GyroReadings>("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_);
+}

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_ );

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;
 }
-