Modifications on HEAR_ROS

hear_ros.cmake

@@ -2,3 +2,28 @@ set(HEAR_ROS_INCLUDE_DIR ${CMAKE_CURRENT_LIST_DIR}/include)
 set(HEAR_ROS_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src)
 
 file(GLOB HEAR_ROS_SRCs ${HEAR_ROS_SOURCE_DIR}/*.cpp)
+
+if(PX4_DEPS)
+    find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        hear_msgs
+        mavros_msgs
+        std_srvs
+        geometry_msgs
+        tf2_geometry_msgs
+        tf2
+        tf2_ros
+    )
+else()
+    find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        hear_msgs
+        std_srvs
+        geometry_msgs
+        tf2_geometry_msgs
+        tf2
+        tf2_ros
+    )
+endif()
+
+set(HEAR_ROS_LIBS ${catkin_LIBRARIES})

include/HEAR_ROS/ROSUnit_BoolClnt.hpp

@@ -0,0 +1,22 @@
+#ifndef ROSUNIT_BOOLCLNT
+#define ROSUNIT_BOOLCLNT
+
+#include <ros/ros.h>
+#include <hear_msgs/set_bool.h>
+#include "HEAR_core/DataTypes.hpp"
+#include "HEAR_ROS/ROSUnit_Client.hpp"
+#include <string>
+
+namespace HEAR{
+
+class ROSUnitBoolClient: public ROSUnit_Client<bool>{
+
+public:
+    ros::NodeHandle nh_;
+    ROSUnitBoolClient(ros::NodeHandle& nh, std::string);
+    bool process();
+};
+
+}
+
+#endif

include/HEAR_ROS/ROSUnit_BoolSrv.hpp

@@ -12,11 +12,11 @@ class ROSUnit_BoolServer {
 private:
     ros::NodeHandle nh_;
     ros::ServiceServer m_server;
-    UpdateTrigger* ext_trig;
+    ExternalTrigger<BaseMsg>* ext_trig;
     bool srv_callback(hear_msgs::set_bool::Request&, hear_msgs::set_bool::Response&);
 public:
     ROSUnit_BoolServer(ros::NodeHandle&);
-    UpdateTrigger* registerServer(const std::string&);
+    ExternalTrigger<BaseMsg>* registerServer(const std::string&);
 
 };
 

include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp

@@ -0,0 +1,74 @@
+// #ifndef ROSUNIT_BOUNDINGSWITCHSRV_HPP
+// #define ROSUNIT_BOUNDINGSWITCHSRV_HPP
+
+// #include <ros/ros.h>
+// #include <hear_msgs/set_float.h>
+
+// #include "HEAR_core/ExternalTrigger.hpp"
+
+// #include <string>
+// #include <vector>
+// #include <utility>
+
+
+// namespace HEAR{
+
+// class ROSUnit_BoundingSwitchSrv {
+// private:
+//     ExternalTrigger<BaseMsg>* _bounding_trig;
+//     std::vector <std::pair<ExternalTrigger<BaseMsg>*, bool>> _switch_trig;
+//     ros::ServiceServer m_server;
+//     bool srvCallback(hear_msgs::set_float::Request& , hear_msgs::set_float::Response& );
+// public:
+//     ROSUnit_BoundingSwitchSrv(ros::NodeHandle& nh , const std::string& topic_name ){
+//         m_server = nh.advertiseService(topic_name, &ROSUnit_BoundingSwitchSrv::srvCallback, this);
+//         _bounding_trig = new ExternalTrigger<BaseMsg>();
+//     }
+//     ExternalTrigger<BaseMsg>* getBoundingTrig(){ return _bounding_trig;}
+//     ExternalTrigger<BaseMsg>* registerSwitchTrig(bool inverted_logic = false){
+//         auto trig = new ExternalTrigger<BaseMsg>();
+//         _switch_trig.push_back(std::make_pair(trig, inverted_logic));
+//         return trig;
+//     }
+
+// };
+
+// bool ROSUnit_BoundingSwitchSrv::srvCallback(hear_msgs::set_float::Request& req, hear_msgs::set_float::Response& res){
+//     float data = req.data;
+//     if(data <= 0){
+//         BoolMsg msg;
+//         msg.data = false;
+//         _bounding_trig->UpdateCallback((BaseMsg*)&msg);
+//         SwitchMsg sw_msg;
+//         for (const auto& trig : _switch_trig){
+//             if(trig.second){
+//                 sw_msg.sw_state = SWITCH_STATE::ON;
+//             }
+//             else{
+//                 sw_msg.sw_state = SWITCH_STATE::OFF;
+//             }
+//             trig.first->UpdateCallback((BaseMsg*)&sw_msg);
+//         }
+
+//     }
+//     else{
+//         BoolMsg msg;
+//         msg.data = true;
+//         _bounding_trig->UpdateCallback((BaseMsg*)&msg);
+//         SwitchMsg sw_msg;
+//         for (const auto& trig : _switch_trig){
+//             if(trig.second){
+//                 sw_msg.sw_state = SWITCH_STATE::OFF;
+//             }
+//             else{
+//                 sw_msg.sw_state = SWITCH_STATE::ON;
+//             }
+//             trig.first->UpdateCallback((BaseMsg*)&sw_msg);
+//         }
+//     }
+//     return true;
+// }
+
+// }
+
+// #endif

include/HEAR_ROS/ROSUnit_Client.hpp

@@ -0,0 +1,18 @@
+#ifndef ROSUNIT_CLIENT_HPP
+#define ROSUNIT_CLIENT_HPP
+
+#include <ros/ros.h>
+#include "HEAR_core/DataTypes.hpp"
+
+namespace HEAR{
+
+template <typename T>
+class ROSUnit_Client {
+public:
+    T data;
+    ros::ServiceClient m_client;
+    virtual bool process() {return true;};
+};
+}
+
+#endif

include/HEAR_ROS/ROSUnit_EmptyClnt.hpp

@@ -1,19 +1,19 @@
-#ifndef ROSUNIT_EMPTYCLNT
-#define ROSUNIT_EMPTYCLNT
+// #ifndef ROSUNIT_EMPTYCLNT
+// #define ROSUNIT_EMPTYCLNT
 
-#include <ros/ros.h>
-#include <std_srvs/Empty.h>
+// #include <ros/ros.h>
+// #include <std_srvs/Empty.h>
+// #include "HEAR_ROS/ROSUnit_Client.hpp"
 
-namespace HEAR{
+// namespace HEAR{
 
-class ROSUnitEmptyClient {
-private:
-    ros::ServiceClient m_client;
-public:
-    ROSUnitEmptyClient(ros::NodeHandle&, std::string);
-    bool process();
-};
+// class ROSUnitEmptyClient: public ROSUnit_Client<bool>{
+// public:
+//     ros::NodeHandle nh_;
+//     ROSUnitEmptyClient(ros::NodeHandle&, std::string);
+//     bool process();
+// };
 
-}
+// }
 
-#endif
+// #endif

include/HEAR_ROS/ROSUnit_Float3Clnt.hpp

@@ -0,0 +1,21 @@
+#ifndef ROSUNIT_FLOAT3CLNT
+#define ROSUNIT_FLOAT3CLNT
+
+#include <ros/ros.h>
+#include <hear_msgs/set_point.h>
+#include "HEAR_core/DataTypes.hpp"
+#include "HEAR_ROS/ROSUnit_Client.hpp"
+#include <string>
+
+namespace HEAR{
+
+class ROSUnitFloat3Client : public ROSUnit_Client<std::vector<float>>{
+public:
+    ros::NodeHandle nh_;
+    ROSUnitFloat3Client(ros::NodeHandle&, std::string);
+    bool process();
+};
+
+}
+
+#endif

include/HEAR_ROS/ROSUnit_Float3Srv.hpp

@@ -0,0 +1,25 @@
+#ifndef ROSUNIT_FLOAT3SRV_HPP
+#define ROSUNIT_FLOAT3SRV_HPP
+
+#include <ros/ros.h>
+#include <hear_msgs/set_point.h>
+#include <string>
+
+#include "HEAR_core/ExternalTrigger.hpp"
+
+namespace HEAR{
+class ROSUnit_Float3Server {
+private:
+    ros::NodeHandle nh_;
+    ros::ServiceServer m_server;
+    ExternalTrigger<BaseMsg>* ext_trig;
+    bool srv_callback(hear_msgs::set_point::Request&, hear_msgs::set_point::Response&);
+public:
+    ROSUnit_Float3Server(ros::NodeHandle&);
+    ExternalTrigger<BaseMsg>* registerServer(const std::string&);
+
+};
+
+}
+
+#endif

include/HEAR_ROS/ROSUnit_FloatArrPub.hpp

@@ -12,7 +12,7 @@ class ROSUnitFloatArrPub : public ROSUnit_Pub{
 private:
 public:
     ROSUnitFloatArrPub(ros::NodeHandle& nh, const std::string& topic_name, int idx){
-        pub_ = nh.advertise<std_msgs::Float32MultiArray>(topic_name, 10, true);
+        pub_ = nh.advertise<std_msgs::Float32MultiArray>(topic_name, 1, true);
         _input_port = new InputPort<std::vector<float>>(idx, 0);
         id_ = idx;
     }

include/HEAR_ROS/ROSUnit_FloatArrSub.hpp

@@ -0,0 +1,35 @@
+#ifndef ROSUNIT_FLOATARRSUB_HPP
+#define ROSUNIT_FLOATARRSUB_HPP
+
+#include "HEAR_ROS/ROSUnit_Sub.hpp"
+#include "std_msgs/Float32MultiArray.h"
+
+#include <vector>
+
+namespace HEAR{
+
+class ROSUnitFloatArrSub : public ROSUnit_Sub{
+private:
+    void callback(const std_msgs::Float32MultiArray::ConstPtr& msg){
+        ((OutputPort<std::vector<float>>*)_output_port)->write(msg->data);
+    }
+public:
+    ROSUnitFloatArrSub(ros::NodeHandle& nh, const std::string& topic_name, int idx){
+        sub_ = nh.subscribe<std_msgs::Float32MultiArray>(topic_name, 1, &ROSUnitFloatArrSub::callback, this);
+        _output_port = new OutputPort<std::vector<float>>(idx, 0);
+        std::vector<float> _commands {0,0,0,0,0,0};
+        for (int i = 0; i < 6; i++){
+            _commands[i] = 1000.0;
+        }
+
+        ((OutputPort<std::vector<float>>*)_output_port)->write(_commands);
+        id_ = idx;
+    }
+
+    TYPE getType(){ return TYPE::FloatVec;}
+
+};
+
+}
+
+#endif

include/HEAR_ROS/ROSUnit_FloatClnt.hpp

@@ -0,0 +1,21 @@
+#ifndef ROSUNIT_FLOATCLNT
+#define ROSUNIT_FLOATCLNT
+
+#include <ros/ros.h>
+#include <hear_msgs/set_float.h>
+#include "HEAR_core/DataTypes.hpp"
+#include "HEAR_ROS/ROSUnit_Client.hpp"
+#include <string>
+
+namespace HEAR{
+
+class ROSUnitFloatClient : public ROSUnit_Client<float>{
+public:
+    ros::NodeHandle nh_;
+    ROSUnitFloatClient(ros::NodeHandle&, std::string);
+    bool process();
+};
+
+}
+
+#endif

include/HEAR_ROS/ROSUnit_FloatPub.hpp

@@ -11,7 +11,7 @@ namespace HEAR{
 class ROSUnitFloatPub : public ROSUnit_Pub{
 public:
     ROSUnitFloatPub(ros::NodeHandle& nh, const std::string& topic_name, int idx) {
-        pub_ = nh.advertise<std_msgs::Float32>(topic_name, 10);
+        pub_ = nh.advertise<std_msgs::Float32>(topic_name, 1);
         _input_port = new InputPort<float>(idx, 0);
         id_ = idx; 
     }

include/HEAR_ROS/ROSUnit_FloatSrv.hpp

@@ -0,0 +1,25 @@
+#ifndef ROSUNIT_FLOATSRV_HPP
+#define ROSUNIT_FLOATSRV_HPP
+
+#include <ros/ros.h>
+#include <hear_msgs/set_float.h>
+#include <string>
+
+#include "HEAR_core/ExternalTrigger.hpp"
+
+namespace HEAR{
+class ROSUnit_FloatServer {
+private:
+    ros::NodeHandle nh_;
+    ros::ServiceServer m_server;
+    ExternalTrigger<BaseMsg>* ext_trig;
+    bool srv_callback(hear_msgs::set_float::Request&, hear_msgs::set_float::Response&);
+public:
+    ROSUnit_FloatServer(ros::NodeHandle&);
+    ExternalTrigger<BaseMsg>* registerServer(const std::string&);
+
+};
+
+}
+
+#endif

include/HEAR_ROS/ROSUnit_FloatSub.hpp

@@ -15,7 +15,7 @@ class ROSUnitFloatSub : public ROSUnit_Sub{
 };
 
 ROSUnitFloatSub::ROSUnitFloatSub(ros::NodeHandle& nh, const std::string& topic, int idx){
-    sub_ = nh.subscribe<std_msgs::Float32>(topic, 10, &ROSUnitFloatSub::callback, this);
+    sub_ = nh.subscribe<std_msgs::Float32>(topic, 1, &ROSUnitFloatSub::callback, this);
     _output_port = new OutputPort<float>(idx, 0);
 
     ((OutputPort<float>*)_output_port)->write(0);

include/HEAR_ROS/ROSUnit_IntClnt.hpp

@@ -0,0 +1,22 @@
+#ifndef ROSUNIT_INTCLNT
+#define ROSUNIT_INTCLNT
+
+#include <ros/ros.h>
+#include "HEAR_core/DataTypes.hpp"
+#include <hear_msgs/set_int.h>
+#include "HEAR_ROS/ROSUnit_Client.hpp"
+#include <string>
+
+namespace HEAR{
+
+class ROSUnitIntClient: public ROSUnit_Client<int>{
+
+public:
+    ros::NodeHandle nh_;
+    ROSUnitIntClient(ros::NodeHandle& nh, std::string);
+    bool process();
+};
+
+}
+
+#endif