Diff: Roslib/Roslib.cpp

Revision:

0:a4a02499a5f3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Roslib/Roslib.cpp	Mon Oct 26 22:25:16 2020 +0000
@@ -0,0 +1,65 @@
+#ifndef ROSLIB_CPP
+#define ROSLIB_CPP
+#include <ros.h>
+#include <std_msgs/Int16.h>
+#include <robsonema_service/Service_Kicker.h>
+#include <robsonema_service/Service_Setpoint.h>
+#include <geometry_msgs/Quaternion.h>
+#include "Roslib.h"
+#include "Kicker.h"
+#include "Motor.h"
+
+Kicker ros_kick;
+Motor ros_motor;
+
+ros::NodeHandle  nh;
+using robsonema_service::Service_Kicker;
+using robsonema_service::Service_Setpoint;
+
+std_msgs::Int16 proximity_msgs;
+ros::Publisher proximity_publish("/robot/proximity_raw", &proximity_msgs);
+
+
+geometry_msgs::Quaternion rpm;
+ros::Publisher rpm_publish("/robot/rpm_raw", &rpm);
+
+void kicker_callback(const Service_Kicker::Request & req, Service_Kicker::Response & res)
+{
+    ros_kick.kicker(req.kick_speed.data);
+}
+
+void setpoint_callback(const Service_Setpoint::Request & req, Service_Setpoint::Response & res)
+{
+    ros_motor.transmit_i2cmotor(req.setpoint.x,req.setpoint.y,req.setpoint.z,req.setpoint.w);
+}
+
+ros::ServiceServer<Service_Kicker::Request, Service_Kicker::Response> server_kicker("/robot/kicker_service", &kicker_callback);
+ros::ServiceServer<Service_Setpoint::Request, Service_Setpoint::Response> server_setpoint("/robot/setpoint_service", &setpoint_callback);
+
+Roslib::Roslib()
+{
+    nh.initNode();
+    nh.advertise(proximity_publish);
+    nh.advertise(rpm_publish);
+    nh.advertiseService(server_kicker);
+    nh.advertiseService(server_setpoint);
+}
+
+void Roslib::publish_data_proximity(float data)
+{
+    proximity_msgs.data = data;
+    proximity_publish.publish(&proximity_msgs);
+}
+
+void Roslib::ros_routine()
+{
+    ros_motor.read_i2cmotor();
+    rpm.x = ros_motor.RPM[0].data;
+    rpm.y = 0;
+    rpm.z = 0;
+    rpm.w = 0;
+    rpm_publish.publish(&rpm);
+    nh.spinOnce();
+}
+
+#endif
\ No newline at end of file