diff --git a/hear_ros.cmake b/hear_ros.cmake old mode 100644 new mode 100755 index b58d851..fbe1acc --- a/hear_ros.cmake +++ b/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}) \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp new file mode 100755 index 0000000..20f5b69 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -0,0 +1,22 @@ +#ifndef ROSUNIT_BOOLCLNT +#define ROSUNIT_BOOLCLNT + +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include "HEAR_ROS/ROSUnit_Client.hpp" +#include + +namespace HEAR{ + +class ROSUnitBoolClient: public ROSUnit_Client{ + +public: + ros::NodeHandle nh_; + ROSUnitBoolClient(ros::NodeHandle& nh, std::string); + bool process(); +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_BoolSrv.hpp b/include/HEAR_ROS/ROSUnit_BoolSrv.hpp old mode 100644 new mode 100755 index 62229ff..54bbbf5 --- a/include/HEAR_ROS/ROSUnit_BoolSrv.hpp +++ b/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* 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* registerServer(const std::string&); }; diff --git a/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp b/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp new file mode 100755 index 0000000..d05041f --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp @@ -0,0 +1,74 @@ +// #ifndef ROSUNIT_BOUNDINGSWITCHSRV_HPP +// #define ROSUNIT_BOUNDINGSWITCHSRV_HPP + +// #include +// #include + +// #include "HEAR_core/ExternalTrigger.hpp" + +// #include +// #include +// #include + + +// namespace HEAR{ + +// class ROSUnit_BoundingSwitchSrv { +// private: +// ExternalTrigger* _bounding_trig; +// std::vector *, 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(); +// } +// ExternalTrigger* getBoundingTrig(){ return _bounding_trig;} +// ExternalTrigger* registerSwitchTrig(bool inverted_logic = false){ +// auto trig = new ExternalTrigger(); +// _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 diff --git a/include/HEAR_ROS/ROSUnit_Client.hpp b/include/HEAR_ROS/ROSUnit_Client.hpp new file mode 100755 index 0000000..120cc60 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_Client.hpp @@ -0,0 +1,18 @@ +#ifndef ROSUNIT_CLIENT_HPP +#define ROSUNIT_CLIENT_HPP + +#include +#include "HEAR_core/DataTypes.hpp" + +namespace HEAR{ + +template +class ROSUnit_Client { +public: + T data; + ros::ServiceClient m_client; + virtual bool process() {return true;}; +}; +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp old mode 100644 new mode 100755 index 63628db..b90c545 --- a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp @@ -1,19 +1,19 @@ -#ifndef ROSUNIT_EMPTYCLNT -#define ROSUNIT_EMPTYCLNT +// #ifndef ROSUNIT_EMPTYCLNT +// #define ROSUNIT_EMPTYCLNT -#include -#include +// #include +// #include +// #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{ +// public: +// ros::NodeHandle nh_; +// ROSUnitEmptyClient(ros::NodeHandle&, std::string); +// bool process(); +// }; -} +// } -#endif \ No newline at end of file +// #endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_Float3Clnt.hpp b/include/HEAR_ROS/ROSUnit_Float3Clnt.hpp new file mode 100644 index 0000000..ef45e1f --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_Float3Clnt.hpp @@ -0,0 +1,21 @@ +#ifndef ROSUNIT_FLOAT3CLNT +#define ROSUNIT_FLOAT3CLNT + +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include "HEAR_ROS/ROSUnit_Client.hpp" +#include + +namespace HEAR{ + +class ROSUnitFloat3Client : public ROSUnit_Client>{ +public: + ros::NodeHandle nh_; + ROSUnitFloat3Client(ros::NodeHandle&, std::string); + bool process(); +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_Float3Srv.hpp b/include/HEAR_ROS/ROSUnit_Float3Srv.hpp new file mode 100644 index 0000000..bdb42cd --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_Float3Srv.hpp @@ -0,0 +1,25 @@ +#ifndef ROSUNIT_FLOAT3SRV_HPP +#define ROSUNIT_FLOAT3SRV_HPP + +#include +#include +#include + +#include "HEAR_core/ExternalTrigger.hpp" + +namespace HEAR{ +class ROSUnit_Float3Server { +private: + ros::NodeHandle nh_; + ros::ServiceServer m_server; + ExternalTrigger* ext_trig; + bool srv_callback(hear_msgs::set_point::Request&, hear_msgs::set_point::Response&); +public: + ROSUnit_Float3Server(ros::NodeHandle&); + ExternalTrigger* registerServer(const std::string&); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp old mode 100644 new mode 100755 index a008e65..7a01566 --- a/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp +++ b/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(topic_name, 10, true); + pub_ = nh.advertise(topic_name, 1, true); _input_port = new InputPort>(idx, 0); id_ = idx; } diff --git a/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp new file mode 100755 index 0000000..034a426 --- /dev/null +++ b/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 + +namespace HEAR{ + +class ROSUnitFloatArrSub : public ROSUnit_Sub{ +private: + void callback(const std_msgs::Float32MultiArray::ConstPtr& msg){ + ((OutputPort>*)_output_port)->write(msg->data); + } +public: + ROSUnitFloatArrSub(ros::NodeHandle& nh, const std::string& topic_name, int idx){ + sub_ = nh.subscribe(topic_name, 1, &ROSUnitFloatArrSub::callback, this); + _output_port = new OutputPort>(idx, 0); + std::vector _commands {0,0,0,0,0,0}; + for (int i = 0; i < 6; i++){ + _commands[i] = 1000.0; + } + + ((OutputPort>*)_output_port)->write(_commands); + id_ = idx; + } + + TYPE getType(){ return TYPE::FloatVec;} + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp new file mode 100755 index 0000000..655269c --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp @@ -0,0 +1,21 @@ +#ifndef ROSUNIT_FLOATCLNT +#define ROSUNIT_FLOATCLNT + +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include "HEAR_ROS/ROSUnit_Client.hpp" +#include + +namespace HEAR{ + +class ROSUnitFloatClient : public ROSUnit_Client{ +public: + ros::NodeHandle nh_; + ROSUnitFloatClient(ros::NodeHandle&, std::string); + bool process(); +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_FloatPub.hpp b/include/HEAR_ROS/ROSUnit_FloatPub.hpp old mode 100644 new mode 100755 index 9cc1b92..cba7ac3 --- a/include/HEAR_ROS/ROSUnit_FloatPub.hpp +++ b/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(topic_name, 10); + pub_ = nh.advertise(topic_name, 1); _input_port = new InputPort(idx, 0); id_ = idx; } diff --git a/include/HEAR_ROS/ROSUnit_FloatSrv.hpp b/include/HEAR_ROS/ROSUnit_FloatSrv.hpp new file mode 100755 index 0000000..252ca29 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_FloatSrv.hpp @@ -0,0 +1,25 @@ +#ifndef ROSUNIT_FLOATSRV_HPP +#define ROSUNIT_FLOATSRV_HPP + +#include +#include +#include + +#include "HEAR_core/ExternalTrigger.hpp" + +namespace HEAR{ +class ROSUnit_FloatServer { +private: + ros::NodeHandle nh_; + ros::ServiceServer m_server; + ExternalTrigger* ext_trig; + bool srv_callback(hear_msgs::set_float::Request&, hear_msgs::set_float::Response&); +public: + ROSUnit_FloatServer(ros::NodeHandle&); + ExternalTrigger* registerServer(const std::string&); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_FloatSub.hpp b/include/HEAR_ROS/ROSUnit_FloatSub.hpp old mode 100644 new mode 100755 index 8dd0e34..7f0b642 --- a/include/HEAR_ROS/ROSUnit_FloatSub.hpp +++ b/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(topic, 10, &ROSUnitFloatSub::callback, this); + sub_ = nh.subscribe(topic, 1, &ROSUnitFloatSub::callback, this); _output_port = new OutputPort(idx, 0); ((OutputPort*)_output_port)->write(0); diff --git a/include/HEAR_ROS/ROSUnit_Heartbeat.hpp b/include/HEAR_ROS/ROSUnit_Heartbeat.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_IntClnt.hpp b/include/HEAR_ROS/ROSUnit_IntClnt.hpp new file mode 100755 index 0000000..9327ee6 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_IntClnt.hpp @@ -0,0 +1,22 @@ +#ifndef ROSUNIT_INTCLNT +#define ROSUNIT_INTCLNT + +#include +#include "HEAR_core/DataTypes.hpp" +#include +#include "HEAR_ROS/ROSUnit_Client.hpp" +#include + +namespace HEAR{ + +class ROSUnitIntClient: public ROSUnit_Client{ + +public: + ros::NodeHandle nh_; + ROSUnitIntClient(ros::NodeHandle& nh, std::string); + bool process(); +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_IntSrv.hpp b/include/HEAR_ROS/ROSUnit_IntSrv.hpp new file mode 100755 index 0000000..24a2e86 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_IntSrv.hpp @@ -0,0 +1,25 @@ +#ifndef ROSUNIT_INTSRV_HPP +#define ROSUNIT_INTSRV_HPP + +#include +#include +#include + +#include "HEAR_core/ExternalTrigger.hpp" + +namespace HEAR{ +class ROSUnit_IntServer { +private: + ros::NodeHandle nh_; + ros::ServiceServer m_server; + ExternalTrigger* ext_trig; + bool srv_callback(hear_msgs::set_int::Request&, hear_msgs::set_int::Response&); +public: + ROSUnit_IntServer(ros::NodeHandle&); + ExternalTrigger* registerServer(const std::string&); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp b/include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp new file mode 100755 index 0000000..a51f4dc --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp @@ -0,0 +1,104 @@ +// #ifndef ROSUNIT_MULTIPORTSWITCHSRV_HPP +// #define ROSUNIT_MULTIPORTSWITCHSRV_HPP + +// #include +// #include + +// #include "HEAR_core/ExternalTrigger.hpp" + +// #include +// #include +// #include + + +// namespace HEAR{ + +// class ROSUnit_MultiportSwitchSrv { +// private: +// ExternalTrigger* _pid_trig; +// ExternalTrigger* _mrft_trig; +// ExternalTrigger* _bounding_trig; +// std::vector *, bool>> _switch_trig; +// ros::ServiceServer m_server; +// bool srvCallback(hear_msgs::set_float::Request& , hear_msgs::set_float::Response& ); +// public: +// ROSUnit_MultiportSwitchSrv(ros::NodeHandle& nh , const std::string& topic_name ){ +// m_server = nh.advertiseService(topic_name, &ROSUnit_MultiportSwitchSrv::srvCallback, this); +// _pid_trig = new ExternalTrigger(); +// _mrft_trig = new ExternalTrigger(); +// _bounding_trig = new ExternalTrigger(); +// } +// ExternalTrigger* getMRFTTrig(){ return _mrft_trig;} +// ExternalTrigger* getPIDTrig(){ return _pid_trig;} +// ExternalTrigger* getBoundingTrig(){ return _bounding_trig;} +// ExternalTrigger* registerSwitchTrig(bool inverted_logic = false){ +// auto trig = new ExternalTrigger(); +// _switch_trig.push_back(std::make_pair(trig, inverted_logic)); +// return trig; +// } + +// }; + +// bool ROSUnit_MultiportSwitchSrv::srvCallback(hear_msgs::set_float::Request& req, hear_msgs::set_float::Response& res){ +// float data = req.data; +// if(data == 1){ +// BoolMsg msg; +// msg.data = false; +// _pid_trig->UpdateCallback((BaseMsg*)&msg); +// _bounding_trig->UpdateCallback((BaseMsg*)&msg); +// msg.data = true; +// _mrft_trig->UpdateCallback((BaseMsg*)&msg); +// MultiportSwitchMsg sw_msg; +// for (const auto& trig : _switch_trig){ +// if(trig.second){ +// sw_msg.sw_state = MULTIPORT_SWITCH_STATE::ON3; +// } +// else{ +// sw_msg.sw_state = MULTIPORT_SWITCH_STATE::ON1; +// } +// trig.first->UpdateCallback((BaseMsg*)&sw_msg); +// } + +// } +// else if (data == 2){ +// BoolMsg msg; +// msg.data = false; +// _mrft_trig->UpdateCallback((BaseMsg*)&msg); +// _pid_trig->UpdateCallback((BaseMsg*)&msg); +// msg.data = true; +// _bounding_trig->UpdateCallback((BaseMsg*)&msg); +// MultiportSwitchMsg sw_msg; +// for (const auto& trig : _switch_trig){ +// if(trig.second){ +// sw_msg.sw_state = MULTIPORT_SWITCH_STATE::ON3; +// } +// else{ +// sw_msg.sw_state = MULTIPORT_SWITCH_STATE::ON2; +// } +// trig.first->UpdateCallback((BaseMsg*)&sw_msg); +// } +// } +// else{ +// BoolMsg msg; +// msg.data = false; +// _mrft_trig->UpdateCallback((BaseMsg*)&msg); +// _bounding_trig->UpdateCallback((BaseMsg*)&msg); +// msg.data = true; +// _pid_trig->UpdateCallback((BaseMsg*)&msg); +// MultiportSwitchMsg sw_msg; +// for (const auto& trig : _switch_trig){ +// if(trig.second){ +// sw_msg.sw_state = MULTIPORT_SWITCH_STATE::ON1; +// } +// else{ +// sw_msg.sw_state = MULTIPORT_SWITCH_STATE::ON3; +// } +// trig.first->UpdateCallback((BaseMsg*)&sw_msg); +// } +// } +// return true; +// } + +// } + +// #endif diff --git a/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp b/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp new file mode 100755 index 0000000..fe0c1c8 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp @@ -0,0 +1,39 @@ +#ifndef ROSUNIT_PX4ANGULARVELOCITYPROVIDER_HPP +#define ROSUNIT_PX4ANGULARVELOCITYPROVIDER_HPP +#ifdef PX4 + +#include + +#include "ros/ros.h" +#include + +#include + +#include "HEAR_core/Block.hpp" +#include "HEAR_core/ExternalPort.hpp" +#include "HEAR_core/Vector3D.hpp" +#include "HEAR_core/ExternalTrigger.hpp" + +namespace HEAR{ + +class ROSUnit_PX4AngularVelocittyProvider{ +private: + ros::NodeHandle nh_; + ros::Subscriber px4_ang_vel_sub; + ros::ServiceServer m_server; + + ExternalOutputPort>* imu_angular_rt_port; + #ifdef PX4 + void callback_angular_vel(const mavros_msgs::VehicleAngularVelocity::ConstPtr&); + #endif + +public: + void process(){} + ROSUnit_PX4AngularVelocittyProvider(ros::NodeHandle& nh); + ~ROSUnit_PX4AngularVelocittyProvider(){} + ExternalOutputPort>* registerImuAngularRate(std::string t_name); +}; + +} +#endif +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_PointPub.hpp b/include/HEAR_ROS/ROSUnit_PointPub.hpp old mode 100644 new mode 100755 index 1509220..9f9c0d0 --- a/include/HEAR_ROS/ROSUnit_PointPub.hpp +++ b/include/HEAR_ROS/ROSUnit_PointPub.hpp @@ -10,7 +10,7 @@ namespace HEAR{ class ROSUnitPointPub : public ROSUnit_Pub{ public: ROSUnitPointPub(ros::NodeHandle& nh, const std::string& topic_name, int idx){ - pub_ = nh.advertise(topic_name, 10); + pub_ = nh.advertise(topic_name, 1); _input_port = new InputPort>(idx, 0); id_ = idx; } diff --git a/include/HEAR_ROS/ROSUnit_PointSub.hpp b/include/HEAR_ROS/ROSUnit_PointSub.hpp old mode 100644 new mode 100755 index 6142074..eda858c --- a/include/HEAR_ROS/ROSUnit_PointSub.hpp +++ b/include/HEAR_ROS/ROSUnit_PointSub.hpp @@ -15,7 +15,7 @@ class ROSUnitPointSub : public ROSUnit_Sub{ }; ROSUnitPointSub::ROSUnitPointSub (ros::NodeHandle& nh, const std::string& topic, int idx){ - sub_ = nh.subscribe(topic, 10, &ROSUnitPointSub::callback, this, ros::TransportHints().tcpNoDelay()); + sub_ = nh.subscribe(topic, 1, &ROSUnitPointSub::callback, this, ros::TransportHints().tcpNoDelay()); _output_port = new OutputPort>(idx, 0); id_ = idx; } diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp old mode 100644 new mode 100755 index 49a6435..f385733 --- a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp @@ -4,9 +4,15 @@ #include #include "ros/ros.h" +#ifdef PX4 + #include + #include +#endif #include #include #include +#include +#include #include #include @@ -22,26 +28,42 @@ namespace HEAR{ class ROSUnit_PoseProvider{ private: ros::NodeHandle nh_; - ros::Subscriber opti_sub, xsens_ori_sub, xsens_ang_vel_sub, xsens_free_acc_sub; + ros::Subscriber opti_sub, vision_sub, px4_pos_sub, px4_vel_sub, px4_ori_sub, px4_ang_vel_sub, xsens_ori_sub, xsens_ang_vel_sub, xsens_free_acc_sub; ros::ServiceServer m_server; ExternalOutputPort>* opti_pos_port; ExternalOutputPort>* opti_vel_port; ExternalOutputPort>* opti_ori_port; + ExternalOutputPort>* px4_pos_port; + ExternalOutputPort>* px4_vel_port; + ExternalOutputPort>* px4_ori_port; + ExternalOutputPort>* vision_pos_port; + ExternalOutputPort>* vision_vel_port; + ExternalOutputPort>* vision_ori_port; ExternalOutputPort>* imu_ori_port; ExternalOutputPort>* imu_acc_port; ExternalOutputPort>* imu_angular_rt_port; + ExternalOutputPort>* px4_imu_angular_rt_port; void callback_opti_pose(const geometry_msgs::PoseStamped::ConstPtr& ); + void callback_vision_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& ); void callback_ori(const geometry_msgs::QuaternionStamped::ConstPtr& ); void callback_free_acc(const geometry_msgs::Vector3Stamped::ConstPtr& ); void callback_angular_vel(const geometry_msgs::Vector3Stamped::ConstPtr&); + #ifdef PX4 + void callback_px4_pose(const geometry_msgs::PoseStamped::ConstPtr& ); + void callback_px4_vel(const geometry_msgs::TwistStamped::ConstPtr& ); + void callback_px4_ori(const mavros_msgs::VehicleAttitude::ConstPtr& ); + void callback_px4_angular_vel(const mavros_msgs::VehicleAngularVelocity::ConstPtr&); + #endif bool srv_callback(hear_msgs::set_float::Request&, hear_msgs::set_float::Response&); - tf2::Matrix3x3 rot_offset; - tf2::Vector3 trans_offset; + tf2::Matrix3x3 rot_offset, rot_offset_vision, rot_offset_PX4_ori, rot_offset_PX4_yaw; + tf2::Vector3 trans_offset, trans_offset_vision; tf2::Vector3 opti_pos, prev_pos, opti_vel, prev_diff, _hold; - ros::Time prevT; + tf2::Vector3 vision_pos, prev_pos_vision, vision_vel, prev_diff_vision, _hold_vision; + ros::Time prevT, prevT_vision; uint8_t first_read = 0; + uint8_t first_read_vision = 0; const float PEAK_THRESH = 0.35; public: @@ -49,6 +71,11 @@ class ROSUnit_PoseProvider{ ROSUnit_PoseProvider(ros::NodeHandle& nh); ~ROSUnit_PoseProvider(){} std::vector>*> registerOptiPose(std::string t_name); + std::vector>*> registerVisionPose(std::string t_name); + ExternalOutputPort>* registerPX4Pose(std::string t_name); + ExternalOutputPort>* registerPX4Vel(std::string t_name); + ExternalOutputPort>* registerPX4ImuOri(std::string t_name); + ExternalOutputPort>* registerPX4ImuAngularRate(std::string t_name); ExternalOutputPort>* registerImuOri(std::string t_name); ExternalOutputPort>* registerImuAngularRate(std::string t_name); ExternalOutputPort>* registerImuAcceleration(std::string t_name); diff --git a/include/HEAR_ROS/ROSUnit_Pub.hpp b/include/HEAR_ROS/ROSUnit_Pub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_QuatPub.hpp b/include/HEAR_ROS/ROSUnit_QuatPub.hpp old mode 100644 new mode 100755 index 43c2453..e414d73 --- a/include/HEAR_ROS/ROSUnit_QuatPub.hpp +++ b/include/HEAR_ROS/ROSUnit_QuatPub.hpp @@ -12,7 +12,7 @@ namespace HEAR{ class ROSUnitQuatPub : public ROSUnit_Pub{ public: ROSUnitQuatPub(ros::NodeHandle& nh, const std::string& topic_name, int idx){ - pub_ = nh.advertise(topic_name, 10); + pub_ = nh.advertise(topic_name, 1); _input_port = new InputPort(idx, 0); id_ = idx; } diff --git a/include/HEAR_ROS/ROSUnit_QuatSub.hpp b/include/HEAR_ROS/ROSUnit_QuatSub.hpp old mode 100644 new mode 100755 index ba5445a..eed5ee0 --- a/include/HEAR_ROS/ROSUnit_QuatSub.hpp +++ b/include/HEAR_ROS/ROSUnit_QuatSub.hpp @@ -17,7 +17,7 @@ class ROSUnitQuatSub : public ROSUnit_Sub{ }; ROSUnitQuatSub::ROSUnitQuatSub (ros::NodeHandle& nh, const std::string& topic, int idx){ - sub_ = nh.subscribe(topic, 10, &ROSUnitQuatSub::callback, this); + sub_ = nh.subscribe(topic, 1, &ROSUnitQuatSub::callback, this); _output_port = new OutputPort(idx, 0); id_ = idx; } diff --git a/include/HEAR_ROS/ROSUnit_ResetSrv.hpp b/include/HEAR_ROS/ROSUnit_ResetSrv.hpp old mode 100644 new mode 100755 index ffc2710..f262778 --- a/include/HEAR_ROS/ROSUnit_ResetSrv.hpp +++ b/include/HEAR_ROS/ROSUnit_ResetSrv.hpp @@ -3,7 +3,7 @@ #define ROSUNIT_RESETSRV_HPP #include -#include +#include #include #include "HEAR_core/ExternalTrigger.hpp" @@ -12,12 +12,12 @@ namespace HEAR{ class ROSUnit_ResetServer { private: ros::NodeHandle nh_; - ResetTrigger* ext_trig; + ExternalTrigger* ext_trig; ros::ServiceServer m_server; - bool srv_callback(std_srvs::EmptyRequest&, std_srvs::EmptyResponse&); + bool srv_callback(hear_msgs::set_int::Request&, hear_msgs::set_int::Response&); public: ROSUnit_ResetServer(ros::NodeHandle&); - ResetTrigger* registerServer(const std::string&); + ExternalTrigger* registerServer(const std::string&); }; diff --git a/include/HEAR_ROS/ROSUnit_SLAM.hpp b/include/HEAR_ROS/ROSUnit_SLAM.hpp old mode 100644 new mode 100755 index 0fa7298..acdc0df --- a/include/HEAR_ROS/ROSUnit_SLAM.hpp +++ b/include/HEAR_ROS/ROSUnit_SLAM.hpp @@ -1,54 +1,54 @@ -#ifndef ROSUNIT_SLAM_HPP -#define ROSUNIT_SLAM_HPP - -#include - -#include "ros/ros.h" -#include "nav_msgs/Odometry.h" -#include "geometry_msgs/PoseStamped.h" -#include "tf2_ros/transform_listener.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.h" - -#include "HEAR_core/Block.hpp" -#include "HEAR_core/Vector3D.hpp" -#include "HEAR_core/ExternalPort.hpp" -#include "hear_msgs/set_bool.h" -#include "tf2/LinearMath/Transform.h" - -namespace HEAR{ - -class ROSUnit_SLAM { -private: - const float PEAK_THRESH = 0.35; - ros::NodeHandle nh_; - ros::Subscriber odom_sub; - ros::ServiceServer set_offset_srv; - tf2_ros::Buffer tf_Buffer; - std::string ref_frame = "map"; - ros::Time prevT; - uint8_t first_read = 0; - InputPort>* pos_inp_port; - InputPort>* ori_inp_port; - - ExternalOutputPort>* pos_out_port; - ExternalOutputPort>* vel_out_port; - ExternalOutputPort>* ori_out_port; - tf2::Transform offset_tf; - tf2::Vector3 slam_pos, prev_pos, slam_vel, prev_diff, _hold; - tf2::Matrix3x3 slam_rot; - bool to_map = false; - - void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg); - bool srv_callback(hear_msgs::set_bool::Request& req, hear_msgs::set_bool::Response& res); - -public: - ROSUnit_SLAM(ros::NodeHandle& nh, bool use_map = false); - ~ROSUnit_SLAM(){} - std::vector>*> registerSLAM(const std::string& t_name); - void connectInputs(OutputPort>* pos_port, OutputPort>* ori_port); +// #ifndef ROSUNIT_SLAM_HPP +// #define ROSUNIT_SLAM_HPP + +// #include + +// #include "ros/ros.h" +// #include "nav_msgs/Odometry.h" +// #include "geometry_msgs/PoseStamped.h" +// #include "tf2_ros/transform_listener.h" +// #include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +// #include "HEAR_core/Block.hpp" +// #include "HEAR_core/Vector3D.hpp" +// #include "HEAR_core/ExternalPort.hpp" +// #include "hear_msgs/set_bool.h" +// #include "tf2/LinearMath/Transform.h" + +// namespace HEAR{ + +// class ROSUnit_SLAM { +// private: +// const float PEAK_THRESH = 0.35; +// ros::NodeHandle nh_; +// ros::Subscriber odom_sub; +// ros::ServiceServer set_offset_srv; +// tf2_ros::Buffer tf_Buffer; +// std::string ref_frame = "map"; +// ros::Time prevT; +// uint8_t first_read = 0; +// InputPort>* pos_inp_port; +// InputPort>* ori_inp_port; + +// ExternalOutputPort>* pos_out_port; +// ExternalOutputPort>* vel_out_port; +// ExternalOutputPort>* ori_out_port; +// tf2::Transform offset_tf; +// tf2::Vector3 slam_pos, prev_pos, slam_vel, prev_diff, _hold; +// tf2::Matrix3x3 slam_rot; +// bool to_map = false; + +// void odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg); +// bool srv_callback(hear_msgs::set_bool::Request& req, hear_msgs::set_bool::Response& res); + +// public: +// ROSUnit_SLAM(ros::NodeHandle& nh, bool use_map = false); +// ~ROSUnit_SLAM(){} +// std::vector>*> registerSLAM(const std::string& t_name); +// void connectInputs(OutputPort>* pos_port, OutputPort>* ori_port); -}; +// }; -} +// } -#endif \ No newline at end of file +// #endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_Sub.hpp b/include/HEAR_ROS/ROSUnit_Sub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp new file mode 100755 index 0000000..5e00d6f --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp @@ -0,0 +1,26 @@ +#ifndef ROSUNIT_UPDATEBOUNDINGSRV_HPP +#define ROSUNIT_UPDATEBOUNDINGSRV_HPP + +#include +#include +#include +#include + +#include "HEAR_core/ExternalTrigger.hpp" + +namespace HEAR{ +class ROSUnit_UpdateBOUNDINGsrv { +private: + ros::NodeHandle nh_; + ros::ServiceServer m_server; + ExternalTrigger* ext_trig; + bool srv_callback(hear_msgs::Update_Controller_Bounding::Request&, hear_msgs::Update_Controller_Bounding::Response&); +public: + ROSUnit_UpdateBOUNDINGsrv(ros::NodeHandle& nh) : nh_(nh) {} + ExternalTrigger* registerServer(const std::string&); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_UpdateBoundingContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateBoundingContClnt.hpp new file mode 100755 index 0000000..246d9d3 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateBoundingContClnt.hpp @@ -0,0 +1,26 @@ +#ifndef ROSUNIT_UPDATEBOUNDINGCONTCLNT_HPP +#define ROSUNIT_UPDATEBOUNDINGCONTCLNT_HPP + + +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include + + + +namespace HEAR{ +class ROSUnit_UpdateBoundingContClnt { +private: + ros::NodeHandle nh_; + ros::ServiceClient m_client_bounding; +public: + bool process(BaseMsg* t_msg); + ROSUnit_UpdateBoundingContClnt(ros::NodeHandle& nh, std::string Drone_Name, std::string Channel); + ~ROSUnit_UpdateBoundingContClnt(); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp new file mode 100755 index 0000000..803ffc3 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -0,0 +1,46 @@ +// #ifndef ROSUNIT_UPDATECONTCLNT_HPP +// #define ROSUNIT_UPDATECONTCLNT_HPP + + +// #include +// #include +// #include +// #include +// #include +// #include +// #include "HEAR_core/DataTypes.hpp" +// #include + + + +// namespace HEAR{ +// class ROSUnit_UpdateContClnt { +// private: +// ros::NodeHandle nh_; +// ros::ServiceClient m_client_pid_x; +// ros::ServiceClient m_client_pid_y; +// ros::ServiceClient m_client_pid_z; +// ros::ServiceClient m_client_pid_roll; +// ros::ServiceClient m_client_pid_pitch; +// ros::ServiceClient m_client_pid_yaw; +// ros::ServiceClient m_client_pid_yaw_rate; +// ros::ServiceClient m_client_mrft_x; +// ros::ServiceClient m_client_mrft_y; +// ros::ServiceClient m_client_mrft_z; +// ros::ServiceClient m_client_mrft_roll; +// ros::ServiceClient m_client_mrft_pitch; +// ros::ServiceClient m_client_mrft_yaw; +// ros::ServiceClient m_client_mrft_yaw_rate; +// ros::ServiceClient m_client_bounding_x; +// ros::ServiceClient m_client_bounding_y; +// ros::ServiceClient m_client_bounding_z; +// public: +// bool process(UpdateMsg* t_msg, BLOCK_ID ID); +// ROSUnit_UpdateContClnt(ros::NodeHandle& nh, std::string Drone_Name); +// ~ROSUnit_UpdateContClnt(); + +// }; + +// } + +// #endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp old mode 100644 new mode 100755 index 27a0bc1..37353a9 --- a/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp @@ -13,11 +13,11 @@ class ROSUnit_UpdateContSrv { private: ros::NodeHandle nh_; ros::ServiceServer m_server; - UpdateTrigger* ext_trig; + ExternalTrigger* ext_trig; bool srv_callback(hear_msgs::Update_Controller_PID::Request&, hear_msgs::Update_Controller_PID::Response&); public: ROSUnit_UpdateContSrv(ros::NodeHandle& nh) : nh_(nh) {} - UpdateTrigger* registerServer(const std::string&); + ExternalTrigger* registerServer(const std::string&); }; diff --git a/include/HEAR_ROS/ROSUnit_UpdateMRFTContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateMRFTContClnt.hpp new file mode 100755 index 0000000..f302fc9 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateMRFTContClnt.hpp @@ -0,0 +1,27 @@ +#ifndef ROSUNIT_UPDATEMRFTCONTCLNT_HPP +#define ROSUNIT_UPDATEMRFTCONTCLNT_HPP + + +#include +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include + + + +namespace HEAR{ +class ROSUnit_UpdateMRFTContClnt { +private: + ros::NodeHandle nh_; + ros::ServiceClient m_client_mrft; +public: + bool process(BaseMsg* t_msg); + ROSUnit_UpdateMRFTContClnt(ros::NodeHandle& nh, std::string Drone_Name, std::string Channel); + ~ROSUnit_UpdateMRFTContClnt(); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp old mode 100644 new mode 100755 index 39aeaba..85a40be --- a/include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp @@ -13,11 +13,11 @@ class ROSUnit_UpdateMRFTsrv { private: ros::NodeHandle nh_; ros::ServiceServer m_server; - UpdateTrigger* ext_trig; + ExternalTrigger* ext_trig; bool srv_callback(hear_msgs::Update_Controller_MRFT::Request&, hear_msgs::Update_Controller_MRFT::Response&); public: ROSUnit_UpdateMRFTsrv(ros::NodeHandle& nh) : nh_(nh) {} - UpdateTrigger* registerServer(const std::string&); + ExternalTrigger* registerServer(const std::string&); }; diff --git a/include/HEAR_ROS/ROSUnit_UpdatePIDContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdatePIDContClnt.hpp new file mode 100755 index 0000000..b8bfcd3 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdatePIDContClnt.hpp @@ -0,0 +1,27 @@ +#ifndef ROSUNIT_UPDATEPIDCONTCLNT_HPP +#define ROSUNIT_UPDATEPIDCONTCLNT_HPP + + +#include +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include + + + +namespace HEAR{ +class ROSUnit_UpdatePIDContClnt { +private: + ros::NodeHandle nh_; + ros::ServiceClient m_client_pid; +public: + bool process(BaseMsg* t_msg); + ROSUnit_UpdatePIDContClnt(ros::NodeHandle& nh, std::string Drone_Name, std::string Channel); + ~ROSUnit_UpdatePIDContClnt(); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_UpdateTrajectoryClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateTrajectoryClnt.hpp new file mode 100755 index 0000000..93b22e5 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateTrajectoryClnt.hpp @@ -0,0 +1,26 @@ +#ifndef ROSUNIT_UPDATETRAJECTORYCLNT_HPP +#define ROSUNIT_UPDATETRAJECTORYCLNT_HPP + + +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include + + + +namespace HEAR{ +class ROSUnit_UpdateTrajectoryClnt { +private: + ros::NodeHandle nh_; + ros::ServiceClient m_client; +public: + bool process(BaseMsg* t_msg); + ROSUnit_UpdateTrajectoryClnt(ros::NodeHandle& nh, std::string Drone_Name); + ~ROSUnit_UpdateTrajectoryClnt(); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp new file mode 100755 index 0000000..ddcfa14 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp @@ -0,0 +1,26 @@ +#ifndef ROSUNIT_UPDATETRAJECTORYSRV_HPP +#define ROSUNIT_UPDATETRAJECTORYSRV_HPP + +#include +#include +#include +#include + +#include "HEAR_core/ExternalTrigger.hpp" + +namespace HEAR{ +class ROSUnit_UpdateTrajectorySrv { +private: + ros::NodeHandle nh_; + ros::ServiceServer m_server; + ExternalTrigger* ext_trig; + bool srv_callback(hear_msgs::Update_Trajectory::Request&, hear_msgs::Update_Trajectory::Response&); +public: + ROSUnit_UpdateTrajectorySrv(ros::NodeHandle& nh) : nh_(nh) {} + ExternalTrigger* registerServer(const std::string&); + +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/RosSystem.hpp b/include/HEAR_ROS/RosSystem.hpp old mode 100644 new mode 100755 index 1808d7b..c0629b8 --- a/include/HEAR_ROS/RosSystem.hpp +++ b/include/HEAR_ROS/RosSystem.hpp @@ -7,17 +7,23 @@ #include "HEAR_core/System.hpp" #include "HEAR_ROS/ROSUnit_Pub.hpp" #include "HEAR_ROS/ROSUnit_FloatArrPub.hpp" +#include "HEAR_ROS/ROSUnit_FloatArrSub.hpp" #include "HEAR_ROS/ROSUnit_FloatPub.hpp" #include "HEAR_ROS/ROSUnit_PointPub.hpp" #include "HEAR_ROS/ROSUnit_QuatPub.hpp" #include "HEAR_ROS/ROSUnit_ResetSrv.hpp" #include "HEAR_ROS/ROSUnit_UpdateContSrv.hpp" #include "HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp" +#include "HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp" +#include "HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp" #include "HEAR_ROS/ROSUnit_BoolSrv.hpp" +#include "HEAR_ROS/ROSUnit_IntSrv.hpp" #include "HEAR_ROS/ROSUnit_Sub.hpp" #include "HEAR_ROS/ROSUnit_PointSub.hpp" #include "HEAR_ROS/ROSUnit_FloatSub.hpp" #include "HEAR_ROS/ROSUnit_QuatSub.hpp" +#include "HEAR_ROS/ROSUnit_FloatSrv.hpp" +#include "HEAR_ROS/ROSUnit_Float3Srv.hpp" #include namespace HEAR{ @@ -34,10 +40,10 @@ class RosSystem : public System { template void connectSub(ROSUnit_Sub* sub, InputPort* port); - ExternalTrigger* createResetTrigger(std::string topic); - ExternalTrigger* createResetTrigger(std::string topic, Block* dest_block); - ExternalTrigger* createUpdateTrigger(UPDATE_MSG_TYPE type, std::string topic); - ExternalTrigger* createUpdateTrigger(UPDATE_MSG_TYPE type, std::string topic, Block* dest_block); + ExternalTrigger* createResetTrigger(std::string topic); + ExternalTrigger* createResetTrigger(std::string topic, AsyncInputPort* dest_AsyncIP); + ExternalTrigger* createUpdateTrigger(MSG_TYPE type, std::string topic); + ExternalTrigger* createUpdateTrigger(MSG_TYPE type, std::string topic, AsyncInputPort* dest_AsyncIP); void start(); private: ros::NodeHandle nh_; @@ -87,8 +93,11 @@ ROSUnit_Sub* RosSystem::createSub(TYPE d_type, std::string topic_name){ case TYPE::Float : sub = new ROSUnitFloatSub(nh_, topic_name, sub_counter++); break; + case TYPE::FloatVec : + sub = new ROSUnitFloatArrSub(nh_, topic_name, sub_counter++); + break; default: - std::cout <<"invalid subscriber type" <* src_port){ this->createPub(TYPE::Float, topic_name, src_port); } -ExternalTrigger* RosSystem::createResetTrigger(std::string topic){ +ExternalTrigger* RosSystem::createResetTrigger(std::string topic){ auto srv = new ROSUnit_ResetServer(pnh_); auto trig = srv->registerServer(topic); this->addExternalTrigger(trig, topic); return trig; } -ExternalTrigger* RosSystem::createResetTrigger(std::string topic, Block* dest_block){ +ExternalTrigger* RosSystem::createResetTrigger(std::string topic, AsyncInputPort* dest_AsyncIP){ auto trig = this->createResetTrigger(topic); - this->connectExternalTrigger(trig, dest_block); + this->connectExternalTrigger(trig, dest_AsyncIP); return trig; } -ExternalTrigger* RosSystem::createUpdateTrigger(UPDATE_MSG_TYPE type, std::string topic){ +ExternalTrigger* RosSystem::createUpdateTrigger(MSG_TYPE type, std::string topic){ //TODO make class for ROSUnit_Srv - ExternalTrigger* trig; + ExternalTrigger* trig; switch(type){ - case UPDATE_MSG_TYPE::PID_UPDATE :{ + case MSG_TYPE::PID_UPDATE :{ auto srv = new ROSUnit_UpdateContSrv(pnh_); trig = srv->registerServer(topic); break;} - case UPDATE_MSG_TYPE::BOOL_MSG :{ + case MSG_TYPE::BOOL_MSG :{ auto srv = new ROSUnit_BoolServer(pnh_); trig = srv->registerServer(topic); break; } - case UPDATE_MSG_TYPE::MRFT_UPDATE : { + case MSG_TYPE::INT_MSG :{ + auto srv = new ROSUnit_IntServer(pnh_); + trig = srv->registerServer(topic); + break; + } + case MSG_TYPE::MRFT_UPDATE : { auto srv = new ROSUnit_UpdateMRFTsrv(pnh_); trig = srv->registerServer(topic); break; } + case MSG_TYPE::BOUNDINGCTRL_UPDATE : { + auto srv = new ROSUnit_UpdateBOUNDINGsrv(pnh_); + trig = srv->registerServer(topic); + break; + } + case MSG_TYPE::TRAJECTORY_UPDATE : { + auto srv = new ROSUnit_UpdateTrajectorySrv(pnh_); + trig = srv->registerServer(topic); + break; + } + case MSG_TYPE::FLOAT_MSG : { + auto srv = new ROSUnit_FloatServer(pnh_); + trig = srv->registerServer(topic); + break; + } + case MSG_TYPE::FLOAT3_MSG : { + auto srv = new ROSUnit_Float3Server(pnh_); + trig = srv->registerServer(topic); + break; + } default: - return NULL; + ROS_ERROR("RosSystem::createUpdateTrigger: Unknown update message type"); } this->addExternalTrigger(trig, topic); return trig; } -ExternalTrigger* RosSystem::createUpdateTrigger(UPDATE_MSG_TYPE type, std::string topic, Block* dest_block){ +ExternalTrigger* RosSystem::createUpdateTrigger(MSG_TYPE type, std::string topic, AsyncInputPort* dest_AsyncIP){ auto trig = this->createUpdateTrigger(type, topic); - this->connectExternalTrigger(trig, dest_block); + this->connectExternalTrigger(trig, dest_AsyncIP); return trig; } diff --git a/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp b/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp old mode 100644 new mode 100755 index 59ac3dd..806f168 --- a/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp +++ b/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp @@ -1,80 +1,81 @@ -#ifndef ROSUNIT_MRFTSWITCHSRV_HPP -#define ROSUNIT_MRFTSWITCHSRV_HPP +// #ifndef ROSUNIT_MRFTSWITCHSRV_HPP +// #define ROSUNIT_MRFTSWITCHSRV_HPP -#include -#include +// #include +// #include -#include "HEAR_core/ExternalTrigger.hpp" +// #include "HEAR_core/ExternalTrigger.hpp" -#include -#include -#include +// #include +// #include +// #include -namespace HEAR{ +// namespace HEAR{ -class ROSUnit_MRFTSwitchSrv { -private: - UpdateTrigger* _pid_trig; - UpdateTrigger* _mrft_trig; - std::vector > _switch_trig; - ros::ServiceServer m_server; - bool srvCallback(hear_msgs::set_float::Request& , hear_msgs::set_float::Response& ); -public: - ROSUnit_MRFTSwitchSrv(ros::NodeHandle& nh , const std::string& topic_name ){ - m_server = nh.advertiseService(topic_name, &ROSUnit_MRFTSwitchSrv::srvCallback, this); - _pid_trig = new UpdateTrigger(); - _mrft_trig = new UpdateTrigger(); - } - ExternalTrigger* getMRFTTrig(){ return _mrft_trig;} - ExternalTrigger* getPIDTrig(){ return _pid_trig;} - ExternalTrigger* registerSwitchTrig(bool inverted_logic = false){ - auto trig = new UpdateTrigger(); - _switch_trig.push_back(std::make_pair(trig, inverted_logic)); - return trig; - } +// class ROSUnit_MRFTSwitchSrv { +// private: +// ExternalTrigger* _pid_trig; +// ExternalTrigger* _mrft_trig; +// std::vector *, bool>> _switch_trig; +// ros::ServiceServer m_server; +// bool srvCallback(hear_msgs::set_float::Request& , hear_msgs::set_float::Response& ); +// public: +// ROSUnit_MRFTSwitchSrv(ros::NodeHandle& nh , const std::string& topic_name ){ +// m_server = nh.advertiseService(topic_name, &ROSUnit_MRFTSwitchSrv::srvCallback, this); +// _pid_trig = new ExternalTrigger(); +// _mrft_trig = new ExternalTrigger(); +// } +// ExternalTrigger* getMRFTTrig(){ return _mrft_trig;} +// ExternalTrigger* getPIDTrig(){ return _pid_trig;} +// ExternalTrigger* registerSwitchTrig(bool inverted_logic = false){ +// auto trig = new ExternalTrigger(); +// _switch_trig.push_back(std::make_pair(trig, inverted_logic)); +// return trig; +// } -}; +// }; -bool ROSUnit_MRFTSwitchSrv::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; - _mrft_trig->UpdateCallback((UpdateMsg*)&msg); - msg.data = true; - _pid_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); - } +// bool ROSUnit_MRFTSwitchSrv::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; +// _mrft_trig->UpdateCallback((BaseMsg*)&msg); +// msg.data = true; +// _pid_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; - _mrft_trig->UpdateCallback((UpdateMsg*)&msg); - msg.data = false; - _pid_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); - } - } -} +// } +// else{ +// BoolMsg msg; +// msg.data = true; +// _mrft_trig->UpdateCallback((BaseMsg*)&msg); +// msg.data = false; +// _pid_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 \ No newline at end of file +// #endif diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp new file mode 100755 index 0000000..02daca0 --- /dev/null +++ b/src/ROSUnit_BoolClnt.cpp @@ -0,0 +1,15 @@ +#include "HEAR_ROS/ROSUnit_BoolClnt.hpp" + +namespace HEAR{ + +ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name): nh_(nh){ + m_client = nh_.serviceClient(t_name); +} + +bool ROSUnitBoolClient::process(){ + hear_msgs::set_bool msg; + msg.request.data = this->data; + return m_client.call(msg); +} + +} \ No newline at end of file diff --git a/src/ROSUnit_BoolSrv.cpp b/src/ROSUnit_BoolSrv.cpp old mode 100644 new mode 100755 index 75afe95..54cd78b --- a/src/ROSUnit_BoolSrv.cpp +++ b/src/ROSUnit_BoolSrv.cpp @@ -3,10 +3,10 @@ namespace HEAR{ ROSUnit_BoolServer::ROSUnit_BoolServer(ros::NodeHandle &nh) : nh_(nh){ - ext_trig = new UpdateTrigger; + ext_trig = new ExternalTrigger; } -UpdateTrigger* ROSUnit_BoolServer::registerServer(const std::string &service_topic){ +ExternalTrigger* ROSUnit_BoolServer::registerServer(const std::string &service_topic){ m_server = nh_.advertiseService(service_topic, &ROSUnit_BoolServer::srv_callback, this); return ext_trig; } @@ -14,7 +14,7 @@ UpdateTrigger* ROSUnit_BoolServer::registerServer(const std::string &service_top bool ROSUnit_BoolServer::srv_callback(hear_msgs::set_bool::Request& req, hear_msgs::set_bool::Response& res){ BoolMsg msg; msg.data = req.data; - ext_trig->UpdateCallback((UpdateMsg*)&msg); + ext_trig->UpdateCallback((BaseMsg*)&msg); return true; } diff --git a/src/ROSUnit_EmptyClnt.cpp b/src/ROSUnit_EmptyClnt.cpp old mode 100644 new mode 100755 index 7154016..3dcf600 --- a/src/ROSUnit_EmptyClnt.cpp +++ b/src/ROSUnit_EmptyClnt.cpp @@ -1,14 +1,14 @@ -#include "HEAR_ROS/ROSUnit_EmptyClnt.hpp" +// #include "HEAR_ROS/ROSUnit_EmptyClnt.hpp" -namespace HEAR{ +// namespace HEAR{ -ROSUnitEmptyClient::ROSUnitEmptyClient(ros::NodeHandle &nh, std::string t_name){ - m_client = nh.serviceClient(t_name); -} +// ROSUnitEmptyClient::ROSUnitEmptyClient(ros::NodeHandle &nh, std::string t_name) : nh_(nh){ +// m_client = nh_.serviceClient(t_name); +// } -bool ROSUnitEmptyClient::process(){ - std_srvs::Empty msg; - return m_client.call(msg); -} +// bool ROSUnitEmptyClient::process(){ +// std_srvs::Empty msg; +// return m_client.call(msg); +// } -} \ No newline at end of file +// } \ No newline at end of file diff --git a/src/ROSUnit_Float3Clnt.cpp b/src/ROSUnit_Float3Clnt.cpp new file mode 100644 index 0000000..abf8b2d --- /dev/null +++ b/src/ROSUnit_Float3Clnt.cpp @@ -0,0 +1,17 @@ +#include "HEAR_ROS/ROSUnit_Float3Clnt.hpp" + +namespace HEAR{ + +ROSUnitFloat3Client::ROSUnitFloat3Client(ros::NodeHandle &nh, std::string t_name) : nh_(nh){ + m_client = nh_.serviceClient(t_name); +} + +bool ROSUnitFloat3Client::process(){ + hear_msgs::set_point msg; + msg.request.x = this->data[0]; + msg.request.y = this->data[1]; + msg.request.z = this->data[2]; + return m_client.call(msg); +} + +} \ No newline at end of file diff --git a/src/ROSUnit_Float3Srv.cpp b/src/ROSUnit_Float3Srv.cpp new file mode 100644 index 0000000..4cdf20b --- /dev/null +++ b/src/ROSUnit_Float3Srv.cpp @@ -0,0 +1,23 @@ +#include "HEAR_ROS/ROSUnit_Float3Srv.hpp" + +namespace HEAR{ + +ROSUnit_Float3Server::ROSUnit_Float3Server(ros::NodeHandle &nh) : nh_(nh){ + ext_trig = new ExternalTrigger; +} + +ExternalTrigger* ROSUnit_Float3Server::registerServer(const std::string &service_topic){ + m_server = nh_.advertiseService(service_topic, &ROSUnit_Float3Server::srv_callback, this); + return ext_trig; +} + +bool ROSUnit_Float3Server::srv_callback(hear_msgs::set_point::Request& req, hear_msgs::set_point::Response& res){ + Float3Msg msg; + msg.data[0] = req.x; + msg.data[1] = req.y; + msg.data[2] = req.z; + ext_trig->UpdateCallback((BaseMsg*)&msg); + return true; +} + +} \ No newline at end of file diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp new file mode 100755 index 0000000..7ae5a26 --- /dev/null +++ b/src/ROSUnit_FloatClnt.cpp @@ -0,0 +1,15 @@ +#include "HEAR_ROS/ROSUnit_FloatClnt.hpp" + +namespace HEAR{ + +ROSUnitFloatClient::ROSUnitFloatClient(ros::NodeHandle &nh, std::string t_name) : nh_(nh){ + m_client = nh_.serviceClient(t_name); +} + +bool ROSUnitFloatClient::process(){ + hear_msgs::set_float msg; + msg.request.data = this->data; + return m_client.call(msg); +} + +} \ No newline at end of file diff --git a/src/ROSUnit_FloatSrv.cpp b/src/ROSUnit_FloatSrv.cpp new file mode 100755 index 0000000..80e0555 --- /dev/null +++ b/src/ROSUnit_FloatSrv.cpp @@ -0,0 +1,21 @@ +#include "HEAR_ROS/ROSUnit_FloatSrv.hpp" + +namespace HEAR{ + +ROSUnit_FloatServer::ROSUnit_FloatServer(ros::NodeHandle &nh) : nh_(nh){ + ext_trig = new ExternalTrigger; +} + +ExternalTrigger* ROSUnit_FloatServer::registerServer(const std::string &service_topic){ + m_server = nh_.advertiseService(service_topic, &ROSUnit_FloatServer::srv_callback, this); + return ext_trig; +} + +bool ROSUnit_FloatServer::srv_callback(hear_msgs::set_float::Request& req, hear_msgs::set_float::Response& res){ + FloatMsg msg; + msg._value = req.data; + ext_trig->UpdateCallback((BaseMsg*)&msg); + return true; +} + +} \ No newline at end of file diff --git a/src/ROSUnit_IntClnt.cpp b/src/ROSUnit_IntClnt.cpp new file mode 100755 index 0000000..36c3132 --- /dev/null +++ b/src/ROSUnit_IntClnt.cpp @@ -0,0 +1,15 @@ +#include "HEAR_ROS/ROSUnit_IntClnt.hpp" + +namespace HEAR{ + +ROSUnitIntClient::ROSUnitIntClient(ros::NodeHandle &nh, std::string t_name): nh_(nh){ + m_client = nh_.serviceClient(t_name); +} + +bool ROSUnitIntClient::process(){ + hear_msgs::set_int msg; + msg.request.data = this->data; + return m_client.call(msg); +} + +} \ No newline at end of file diff --git a/src/ROSUnit_IntSrv.cpp b/src/ROSUnit_IntSrv.cpp new file mode 100755 index 0000000..d1263fb --- /dev/null +++ b/src/ROSUnit_IntSrv.cpp @@ -0,0 +1,21 @@ +#include "HEAR_ROS/ROSUnit_IntSrv.hpp" + +namespace HEAR{ + +ROSUnit_IntServer::ROSUnit_IntServer(ros::NodeHandle &nh) : nh_(nh){ + ext_trig = new ExternalTrigger; +} + +ExternalTrigger* ROSUnit_IntServer::registerServer(const std::string &service_topic){ + m_server = nh_.advertiseService(service_topic, &ROSUnit_IntServer::srv_callback, this); + return ext_trig; +} + +bool ROSUnit_IntServer::srv_callback(hear_msgs::set_int::Request& req, hear_msgs::set_int::Response& res){ + IntMsg msg; + msg.data = req.data; + ext_trig->UpdateCallback((BaseMsg*)&msg); + return true; +} + +} \ No newline at end of file diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp old mode 100644 new mode 100755 index 2c0fc03..76862be --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -7,36 +7,90 @@ ROSUnit_PoseProvider::ROSUnit_PoseProvider(ros::NodeHandle& nh): nh_(nh){ std::vector>*> ROSUnit_PoseProvider::registerOptiPose(std::string t_name){ - m_server = nh_.advertiseService("/set_height_offset", &ROSUnit_PoseProvider::srv_callback, this); - rot_offset.setRPY(0.0, 0.0, M_PI/2.0); + m_server = nh_.advertiseService("set_height_offset", &ROSUnit_PoseProvider::srv_callback, this); + rot_offset.setRPY(0.0, 0.0, 0.0); trans_offset.setZero(); - opti_pos_port = new ExternalOutputPort>(0); + opti_pos_port = new ExternalOutputPort>(); opti_pos_port->write(Vector3D(0,0,0)); - opti_vel_port = new ExternalOutputPort>(0); + opti_vel_port = new ExternalOutputPort>(); opti_vel_port->write(Vector3D(0,0,0)); - opti_ori_port = new ExternalOutputPort>(0); + opti_ori_port = new ExternalOutputPort>(); opti_ori_port->write(Vector3D(0,0,0)); opti_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_opti_pose, this, ros::TransportHints().tcpNoDelay()); return std::vector>*>{opti_pos_port, opti_vel_port, opti_ori_port}; } +std::vector>*> ROSUnit_PoseProvider::registerVisionPose(std::string t_name){ + rot_offset_vision.setRPY(0.0, 0.0, 0.0); + trans_offset_vision.setZero(); + + vision_pos_port = new ExternalOutputPort>(); + vision_pos_port->write(Vector3D(0,0,0)); + vision_vel_port = new ExternalOutputPort>(); + vision_vel_port->write(Vector3D(0,0,0)); + vision_ori_port = new ExternalOutputPort>(); + vision_ori_port->write(Vector3D(0,0,0)); + vision_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_vision_pose, this, ros::TransportHints().tcpNoDelay()); + return std::vector>*>{vision_pos_port, vision_vel_port, vision_ori_port}; +} + +#ifdef PX4 +ExternalOutputPort>* ROSUnit_PoseProvider::registerPX4Pose(std::string t_name){ + m_server = nh_.advertiseService("set_height_offset", &ROSUnit_PoseProvider::srv_callback, this); + rot_offset.setRPY(0.0, 0.0, 0.0); + trans_offset.setZero(); + + px4_pos_port = new ExternalOutputPort>(); + px4_pos_port->write(Vector3D(0,0,0)); + // px4_vel_port = new ExternalOutputPort>(); + // px4_vel_port->write(Vector3D(0,0,0)); + // px4_ori_port = new ExternalOutputPort>(); + // px4_ori_port->write(Vector3D(0,0,0)); + px4_pos_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_px4_pose, this, ros::TransportHints().tcpNoDelay()); + return px4_pos_port; +} + +ExternalOutputPort>* ROSUnit_PoseProvider::registerPX4Vel(std::string t_name){ + px4_vel_port = new ExternalOutputPort>(); + px4_vel_port->write(Vector3D(0,0,0)); + + px4_vel_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_px4_vel, this, ros::TransportHints().tcpNoDelay()); + return px4_vel_port; +} + +ExternalOutputPort>* ROSUnit_PoseProvider::registerPX4ImuOri(std::string t_name){ + rot_offset_PX4_ori.setRPY(M_PI, 0.0, 0.0); + rot_offset_PX4_yaw.setRPY(0.0, 0.0, M_PI/2.0); + + px4_ori_port = new ExternalOutputPort>(); + px4_ori_port->write(Vector3D(0,0,0)); + px4_ori_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_px4_ori, this, ros::TransportHints().tcpNoDelay()); + return px4_ori_port; +} +ExternalOutputPort>* ROSUnit_PoseProvider::registerPX4ImuAngularRate(std::string t_name){ + px4_imu_angular_rt_port = new ExternalOutputPort>(); + px4_imu_angular_rt_port->write(Vector3D(0,0,0)); + px4_ang_vel_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_px4_angular_vel, this, ros::TransportHints().tcpNoDelay()); + return px4_imu_angular_rt_port; +} +#endif + ExternalOutputPort>* ROSUnit_PoseProvider::registerImuOri(std::string t_name){ - imu_ori_port = new ExternalOutputPort>(0); + imu_ori_port = new ExternalOutputPort>(); imu_ori_port->write(Vector3D(0,0,0)); xsens_ori_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_ori, this, ros::TransportHints().tcpNoDelay()); return imu_ori_port; } ExternalOutputPort>* ROSUnit_PoseProvider::registerImuAngularRate(std::string t_name){ - imu_angular_rt_port = new ExternalOutputPort>(0); + imu_angular_rt_port = new ExternalOutputPort>(); imu_angular_rt_port->write(Vector3D(0,0,0)); xsens_ang_vel_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_angular_vel, this, ros::TransportHints().tcpNoDelay()); return imu_angular_rt_port; } - ExternalOutputPort>* ROSUnit_PoseProvider::registerImuAcceleration(std::string t_name){ - imu_acc_port = new ExternalOutputPort>(0); + imu_acc_port = new ExternalOutputPort>(); imu_acc_port->write(Vector3D(0,0,0)); xsens_free_acc_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_free_acc, this, ros::TransportHints().tcpNoDelay()); return imu_acc_port; @@ -97,6 +151,58 @@ void ROSUnit_PoseProvider::callback_opti_pose(const geometry_msgs::PoseStamped:: opti_ori_port->write(vec_ori); } +void ROSUnit_PoseProvider::callback_vision_pose(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg){ + + tf2::Vector3 vel; + auto pos = tf2::Vector3({msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z}); + auto calib_pos = rot_offset_vision*pos - trans_offset_vision; + + Vector3D vec = {(float)calib_pos.x(), (float)calib_pos.y(), (float)calib_pos.z()}; + + auto R_mat = tf2::Matrix3x3(tf2::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w )); + + R_mat = rot_offset_vision * R_mat * rot_offset_vision.transpose(); + tf2Scalar yaw, pitch, roll; + R_mat.getEulerYPR(yaw, pitch, roll); + + Vector3D vec_ori = {(float)roll, (float)pitch, (float)yaw}; + + // velocity calculation + if(first_read_vision == 0){ + first_read_vision = 1; + prevT_vision = msg->header.stamp; + prev_pos_vision = pos; + vel = tf2::Vector3(0, 0, 0); + prev_diff_vision = vel; + }else{ + auto _dt = (msg->header.stamp - prevT_vision).toSec(); + auto diff = (pos - prev_pos_vision)/_dt; + vel = diff; + if(first_read_vision == 1){ + first_read_vision = 2; + prev_diff_vision = diff; + } + auto d_diff = diff - prev_diff; + if(abs(d_diff.x()) > PEAK_THRESH || abs(d_diff.y()) > PEAK_THRESH || abs(d_diff.z()) > PEAK_THRESH){ + vel = _hold_vision; + } + else{ + _hold_vision = diff; + } + prev_diff_vision = diff; + prev_pos_vision = pos; + prevT_vision = msg->header.stamp; + } + vision_vel = rot_offset_vision*vel; + //////////////////////// + + vision_pos_port->write(vec); + vision_vel_port->write(Vector3D(vision_vel.x(), vision_vel.y(), vision_vel.z())); + vision_ori_port->write(vec_ori); +} + + + void ROSUnit_PoseProvider::callback_ori(const geometry_msgs::QuaternionStamped::ConstPtr& msg){ auto R_mat = tf2::Matrix3x3(tf2::Quaternion(msg->quaternion.x, msg->quaternion.y, msg->quaternion.z, msg->quaternion.w)); @@ -114,6 +220,55 @@ void ROSUnit_PoseProvider::callback_angular_vel(const geometry_msgs::Vector3Stam imu_angular_rt_port->write(vec); } +#ifdef PX4 +void ROSUnit_PoseProvider::callback_px4_pose(const geometry_msgs::PoseStamped::ConstPtr& msg){ + + auto pos = tf2::Vector3({msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}); + auto calib_pos = rot_offset*pos - trans_offset; + + Vector3D vec = {(float)calib_pos.x(), (float)calib_pos.y(), (float)calib_pos.z()}; + + auto R_mat = tf2::Matrix3x3(tf2::Quaternion(msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w )); + + // R_mat = rot_offset * R_mat * rot_offset.transpose(); + tf2Scalar yaw, pitch, roll; + R_mat.getEulerYPR(yaw, pitch, roll); + + Vector3D vec_ori = {(float)roll, (float)pitch, (float)yaw}; + + + px4_pos_port->write(vec); +} +void ROSUnit_PoseProvider::callback_px4_vel(const geometry_msgs::TwistStamped::ConstPtr& msg){ + + auto vel = tf2::Vector3({msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z}); + auto calib_vel = rot_offset*vel - trans_offset; + + Vector3D vec = {(float)calib_vel.x(), (float)calib_vel.y(), (float)calib_vel.z()}; + + + px4_vel_port->write(vec); +} +void ROSUnit_PoseProvider::callback_px4_ori(const mavros_msgs::VehicleAttitude::ConstPtr& msg){ + + auto R_mat = tf2::Matrix3x3(tf2::Quaternion(msg->q_x, msg->q_y, msg->q_z, msg->q_w )); + + R_mat = rot_offset_PX4_yaw * (rot_offset_PX4_ori * R_mat * rot_offset_PX4_ori.transpose()); + tf2Scalar yaw, pitch, roll; + R_mat.getEulerYPR(yaw, pitch, roll); + + Vector3D vec_ori = {(float)roll, (float)pitch, (float)yaw}; + + + px4_ori_port->write(vec_ori); +} +void ROSUnit_PoseProvider::callback_px4_angular_vel(const mavros_msgs::VehicleAngularVelocity::ConstPtr& msg){ + Vector3D vec = {(float)msg->angular_velocity_x, -(float)msg->angular_velocity_y, -(float)msg->angular_velocity_z}; + + px4_imu_angular_rt_port->write(vec); +} +#endif + void ROSUnit_PoseProvider::callback_free_acc(const geometry_msgs::Vector3Stamped::ConstPtr& msg){ Vector3D vec = {(float)msg->vector.x, (float)msg->vector.y, (float)msg->vector.z}; diff --git a/src/ROSUnit_ResetSrv.cpp b/src/ROSUnit_ResetSrv.cpp old mode 100644 new mode 100755 index 9b43b00..f6a883a --- a/src/ROSUnit_ResetSrv.cpp +++ b/src/ROSUnit_ResetSrv.cpp @@ -3,18 +3,19 @@ namespace HEAR{ ROSUnit_ResetServer::ROSUnit_ResetServer(ros::NodeHandle &nh) { - ext_trig = new ResetTrigger; + ext_trig = new ExternalTrigger; } -ResetTrigger* ROSUnit_ResetServer::registerServer(const std::string &service_topic){ +ExternalTrigger* ROSUnit_ResetServer::registerServer(const std::string &service_topic){ m_server = nh_.advertiseService(service_topic, &ROSUnit_ResetServer::srv_callback, this); - return ext_trig; } -bool ROSUnit_ResetServer::srv_callback(std_srvs::EmptyRequest& req, std_srvs::EmptyResponse& res){ - ext_trig->resetCallback(); +bool ROSUnit_ResetServer::srv_callback(hear_msgs::set_int::Request& req, hear_msgs::set_int::Response& res){ + IntMsg msg; + msg.data = req.data; + ext_trig->UpdateCallback((BaseMsg*)&msg); return true; } diff --git a/src/ROSUnit_SLAM.cpp b/src/ROSUnit_SLAM.cpp old mode 100644 new mode 100755 index fb5fdbb..62d6547 --- a/src/ROSUnit_SLAM.cpp +++ b/src/ROSUnit_SLAM.cpp @@ -1,109 +1,109 @@ -#include "HEAR_ROS/ROSUnit_SLAM.hpp" +// #include "HEAR_ROS/ROSUnit_SLAM.hpp" -namespace HEAR{ +// namespace HEAR{ -ROSUnit_SLAM::ROSUnit_SLAM(ros::NodeHandle& nh, bool use_map) : nh_(nh), to_map(use_map){ - pos_inp_port = new InputPort>(0, 0); - ori_inp_port = new InputPort>(0, 0); +// ROSUnit_SLAM::ROSUnit_SLAM(ros::NodeHandle& nh, bool use_map) : nh_(nh), to_map(use_map){ +// pos_inp_port = new InputPort>(0, 0); +// ori_inp_port = new InputPort>(0, 0); - tf2::Matrix3x3 rot; - rot.setIdentity(); - offset_tf = tf2::Transform(rot); - slam_pos = tf2::Vector3(0,0,0); slam_rot.setIdentity(); -} +// tf2::Matrix3x3 rot; +// rot.setIdentity(); +// offset_tf = tf2::Transform(rot); +// slam_pos = tf2::Vector3(0,0,0); slam_rot.setIdentity(); +// } -std::vector>*> ROSUnit_SLAM::registerSLAM(const std::string& t_name){ - set_offset_srv = nh_.advertiseService("/set_map_frame_offset", &ROSUnit_SLAM::srv_callback, this); +// std::vector>*> ROSUnit_SLAM::registerSLAM(const std::string& t_name){ +// set_offset_srv = nh_.advertiseService("/set_map_frame_offset", &ROSUnit_SLAM::srv_callback, this); - odom_sub = nh_.subscribe(t_name, 10, &ROSUnit_SLAM::odom_callback, this); +// odom_sub = nh_.subscribe(t_name, 10, &ROSUnit_SLAM::odom_callback, this); - pos_out_port = new ExternalOutputPort>(0); - vel_out_port = new ExternalOutputPort>(0); - ori_out_port = new ExternalOutputPort>(0); - tf2_ros::TransformListener tf_listener(tf_Buffer); +// pos_out_port = new ExternalOutputPort>(0); +// vel_out_port = new ExternalOutputPort>(0); +// ori_out_port = new ExternalOutputPort>(0); +// tf2_ros::TransformListener tf_listener(tf_Buffer); - return std::vector>*>{pos_out_port, ori_out_port, vel_out_port}; -} +// return std::vector>*>{pos_out_port, ori_out_port, vel_out_port}; +// } -void ROSUnit_SLAM::connectInputs(OutputPort>* pos_port, OutputPort>* ori_port){ - pos_inp_port->connect(pos_port); - ori_inp_port->connect(ori_port); -} +// void ROSUnit_SLAM::connectInputs(OutputPort>* pos_port, OutputPort>* ori_port){ +// pos_inp_port->connect(pos_port); +// ori_inp_port->connect(ori_port); +// } -void ROSUnit_SLAM::odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg){ - tf2::Vector3 pos, vel; - tf2::Matrix3x3 rot; +// void ROSUnit_SLAM::odom_callback(const nav_msgs::Odometry::ConstPtr& odom_msg){ +// tf2::Vector3 pos, vel; +// tf2::Matrix3x3 rot; - geometry_msgs::PoseStamped slam_pose, tf_pose; - slam_pose.header = odom_msg->header; - slam_pose.pose = odom_msg->pose.pose; +// geometry_msgs::PoseStamped slam_pose, tf_pose; +// slam_pose.header = odom_msg->header; +// slam_pose.pose = odom_msg->pose.pose; - if(to_map){ - try{ - tf_Buffer.transform(slam_pose, tf_pose, ref_frame, ros::Duration(0.1)); - } - catch (tf2::TransformException &ex) { - ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught - } - pos = {tf_pose.pose.position.x, tf_pose.pose.position.y, tf_pose.pose.position.z}; - auto ori = tf2::Quaternion(tf_pose.pose.orientation.x, tf_pose.pose.orientation.y, tf_pose.pose.orientation.z, tf_pose.pose.orientation.w); - rot.setRotation(ori); - } - else{ - pos = tf2::Vector3(slam_pose.pose.position.x, slam_pose.pose.position.y, slam_pose.pose.position.z); - rot.setRotation(tf2::Quaternion(slam_pose.pose.orientation.x, slam_pose.pose.orientation.y, slam_pose.pose.orientation.z, slam_pose.pose.orientation.w)); - } - slam_pos = offset_tf*pos; - slam_rot = offset_tf.getBasis()*rot*(offset_tf.getBasis().transpose()); +// if(to_map){ +// try{ +// tf_Buffer.transform(slam_pose, tf_pose, ref_frame, ros::Duration(0.1)); +// } +// catch (tf2::TransformException &ex) { +// ROS_WARN("Failure %s\n", ex.what()); //Print exception which was caught +// } +// pos = {tf_pose.pose.position.x, tf_pose.pose.position.y, tf_pose.pose.position.z}; +// auto ori = tf2::Quaternion(tf_pose.pose.orientation.x, tf_pose.pose.orientation.y, tf_pose.pose.orientation.z, tf_pose.pose.orientation.w); +// rot.setRotation(ori); +// } +// else{ +// pos = tf2::Vector3(slam_pose.pose.position.x, slam_pose.pose.position.y, slam_pose.pose.position.z); +// rot.setRotation(tf2::Quaternion(slam_pose.pose.orientation.x, slam_pose.pose.orientation.y, slam_pose.pose.orientation.z, slam_pose.pose.orientation.w)); +// } +// slam_pos = offset_tf*pos; +// slam_rot = offset_tf.getBasis()*rot*(offset_tf.getBasis().transpose()); - // velocity calculation - if(first_read == 0){ - first_read = 1; - prevT = slam_pose.header.stamp; - prev_pos = pos; - vel = tf2::Vector3(0, 0, 0); - prev_diff = vel; - }else{ - auto _dt = (slam_pose.header.stamp - prevT).toSec(); - auto diff = (pos - prev_pos)/_dt; - vel = diff; - if(first_read == 1){ - first_read = 2; - prev_diff = diff; - } - auto d_diff = diff - prev_diff; - if(abs(d_diff.x()) > PEAK_THRESH || abs(d_diff.y()) > PEAK_THRESH || abs(d_diff.z()) > PEAK_THRESH){ - vel = _hold; - } - else{ - _hold = diff; - } - prev_diff = diff; - prev_pos = pos; - prevT = slam_pose.header.stamp; - } - slam_vel = offset_tf.getBasis()*vel; - //////////////////////// +// // velocity calculation +// if(first_read == 0){ +// first_read = 1; +// prevT = slam_pose.header.stamp; +// prev_pos = pos; +// vel = tf2::Vector3(0, 0, 0); +// prev_diff = vel; +// }else{ +// auto _dt = (slam_pose.header.stamp - prevT).toSec(); +// auto diff = (pos - prev_pos)/_dt; +// vel = diff; +// if(first_read == 1){ +// first_read = 2; +// prev_diff = diff; +// } +// auto d_diff = diff - prev_diff; +// if(abs(d_diff.x()) > PEAK_THRESH || abs(d_diff.y()) > PEAK_THRESH || abs(d_diff.z()) > PEAK_THRESH){ +// vel = _hold; +// } +// else{ +// _hold = diff; +// } +// prev_diff = diff; +// prev_pos = pos; +// prevT = slam_pose.header.stamp; +// } +// slam_vel = offset_tf.getBasis()*vel; +// //////////////////////// - double r, p, y; - slam_rot.getRPY(r, p, y); +// double r, p, y; +// slam_rot.getRPY(r, p, y); - pos_out_port->write(Vector3D(slam_pos.x(), slam_pos.y(), slam_pos.z())); - vel_out_port->write(Vector3D(slam_vel.x(), slam_vel.y(), slam_vel.z())); - ori_out_port->write(Vector3D(r, p, y)); -} +// pos_out_port->write(Vector3D(slam_pos.x(), slam_pos.y(), slam_pos.z())); +// vel_out_port->write(Vector3D(slam_vel.x(), slam_vel.y(), slam_vel.z())); +// ori_out_port->write(Vector3D(r, p, y)); +// } -bool ROSUnit_SLAM::srv_callback(hear_msgs::set_bool::Request& req, hear_msgs::set_bool::Response& res){ - Vector3D angs, trans; - pos_inp_port->read(trans); - ori_inp_port->read(angs); +// bool ROSUnit_SLAM::srv_callback(hear_msgs::set_bool::Request& req, hear_msgs::set_bool::Response& res){ +// Vector3D angs, trans; +// pos_inp_port->read(trans); +// ori_inp_port->read(angs); - tf2::Matrix3x3 rot; - rot.setEulerYPR(angs.z, angs.y, angs.x); - offset_tf.setBasis(rot*(slam_rot.transpose())); - offset_tf.setOrigin(tf2::Vector3(trans.x, trans.y, trans.z) - offset_tf.getBasis()*slam_pos); +// tf2::Matrix3x3 rot; +// rot.setEulerYPR(angs.z, angs.y, angs.x); +// offset_tf.setBasis(rot*(slam_rot.transpose())); +// offset_tf.setOrigin(tf2::Vector3(trans.x, trans.y, trans.z) - offset_tf.getBasis()*slam_pos); - return true; -} +// return true; +// } -} \ No newline at end of file +// } \ No newline at end of file diff --git a/src/ROSUnit_UpdateBOUNDINGsrv.cpp b/src/ROSUnit_UpdateBOUNDINGsrv.cpp new file mode 100755 index 0000000..a10a641 --- /dev/null +++ b/src/ROSUnit_UpdateBOUNDINGsrv.cpp @@ -0,0 +1,22 @@ +#include "HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp" + +namespace HEAR{ + +ExternalTrigger* ROSUnit_UpdateBOUNDINGsrv::registerServer(const std::string &service_topic){ + ext_trig = new ExternalTrigger; + this->m_server = this->nh_.advertiseService(service_topic, &ROSUnit_UpdateBOUNDINGsrv::srv_callback, this); + return ext_trig; +} + +bool ROSUnit_UpdateBOUNDINGsrv::srv_callback(hear_msgs::Update_Controller_Bounding::Request& req, hear_msgs::Update_Controller_Bounding::Response& res){ + BoundingCtrl_UpdateMsg msg; + msg.param.id = (int)req.controller_parameters.id; + msg.param.eps_1 = req.controller_parameters.bounding_eps_1; + msg.param.eps_2 = req.controller_parameters.bounding_eps_2 ; + msg.param.h_o1 = req.controller_parameters.bounding_h_o1; + msg.param.h_o2 = req.controller_parameters.bounding_h_o2; + ext_trig->UpdateCallback((BaseMsg*)&msg); + return true; +} + +} \ No newline at end of file diff --git a/src/ROSUnit_UpdateBoundingContClnt.cpp b/src/ROSUnit_UpdateBoundingContClnt.cpp new file mode 100755 index 0000000..be813d1 --- /dev/null +++ b/src/ROSUnit_UpdateBoundingContClnt.cpp @@ -0,0 +1,37 @@ +#include "HEAR_ROS/ROSUnit_UpdateBoundingContClnt.hpp" + +namespace HEAR{ + +ROSUnit_UpdateBoundingContClnt::ROSUnit_UpdateBoundingContClnt(ros::NodeHandle& t_main_handler, std::string Drone_Name, std::string Channel) : nh_(t_main_handler){ + + m_client_bounding = nh_.serviceClient(Drone_Name + "update_controller/bounding/" + Channel); + +} + +ROSUnit_UpdateBoundingContClnt::~ROSUnit_UpdateBoundingContClnt() { + +} + +bool ROSUnit_UpdateBoundingContClnt::process(BaseMsg* t_msg) { + BoundingCtrl_UpdateMsg* _update_msg = (BoundingCtrl_UpdateMsg*)t_msg; + hear_msgs::Update_Controller_Bounding srv; + srv.request.controller_parameters.id = (int)_update_msg->param.id; + srv.request.controller_parameters.bounding_eps_1 = _update_msg->param.eps_1; + srv.request.controller_parameters.bounding_eps_2 = _update_msg->param.eps_2; + srv.request.controller_parameters.bounding_h_o1 = _update_msg->param.h_o1; + srv.request.controller_parameters.bounding_h_o2 = _update_msg->param.h_o2; + bool success = false; + + success = m_client_bounding.call(srv); + + if (success) { + ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + return true; + } + else { + ROS_ERROR("Failed to call service /update_controller/bounding/"); + return false; + } +} + +} diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp new file mode 100755 index 0000000..ee26b54 --- /dev/null +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -0,0 +1,220 @@ +// #include "HEAR_ROS/ROSUnit_UpdateContClnt.hpp" + +// namespace HEAR{ + +// ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler, std::string Drone_Name) : nh_(t_main_handler){ + +// // PID of the outer loop +// m_client_pid_x = nh_.serviceClient(Drone_Name + "update_controller/pid/x"); +// m_client_pid_y = nh_.serviceClient(Drone_Name + "update_controller/pid/y"); +// m_client_pid_z = nh_.serviceClient(Drone_Name + "update_controller/pid/z"); + +// // PID of the inner loop +// m_client_pid_roll = nh_.serviceClient(Drone_Name + "update_controller/pid/roll"); +// m_client_pid_pitch = nh_.serviceClient(Drone_Name + "update_controller/pid/pitch"); +// m_client_pid_yaw = nh_.serviceClient(Drone_Name + "update_controller/pid/yaw"); +// m_client_pid_yaw_rate = nh_.serviceClient(Drone_Name + "update_controller/pid/yaw_rate"); + +// // MRFT of the outer loop +// m_client_mrft_x = nh_.serviceClient(Drone_Name + "update_controller/mrft/x"); +// m_client_mrft_y = nh_.serviceClient(Drone_Name + "update_controller/mrft/y"); +// m_client_mrft_z = nh_.serviceClient(Drone_Name + "update_controller/mrft/z"); + +// // MRFT of the inner loop +// m_client_mrft_roll = nh_.serviceClient(Drone_Name + "update_controller/mrft/roll"); +// m_client_mrft_pitch = nh_.serviceClient(Drone_Name + "update_controller/mrft/pitch"); +// m_client_mrft_yaw = nh_.serviceClient(Drone_Name + "update_controller/mrft/yaw"); +// m_client_mrft_yaw_rate = nh_.serviceClient(Drone_Name + "update_controller/mrft/yaw_rate"); + +// // Bounding of the outer loop +// m_client_bounding_x = nh_.serviceClient(Drone_Name + "update_controller/bounding/x"); +// m_client_bounding_y = nh_.serviceClient(Drone_Name + "update_controller/bounding/y"); +// m_client_bounding_z = nh_.serviceClient(Drone_Name + "update_controller/bounding/z"); +// } + +// ROSUnit_UpdateContClnt::~ROSUnit_UpdateContClnt() { + +// } + +// bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { +// if(ID == PID) { +// PID_UpdateMsg* _update_msg = (PID_UpdateMsg*)t_msg; +// hear_msgs::Update_Controller_PID srv; +// srv.request.controller_parameters.id = static_cast(_update_msg->param.id); +// srv.request.controller_parameters.pid_kp = _update_msg->param.kp; +// srv.request.controller_parameters.pid_ki = _update_msg->param.ki; +// srv.request.controller_parameters.pid_kd = _update_msg->param.kd; +// srv.request.controller_parameters.pid_kdd = _update_msg->param.kdd; +// srv.request.controller_parameters.pid_anti_windup = _update_msg->param.anti_windup; +// srv.request.controller_parameters.pid_en_pv_derivation = _update_msg->param.en_pv_derivation; +// srv.request.controller_parameters.pid_dt = _update_msg->param.dt; +// bool success = false; + +// switch ((int)_update_msg->param.id) +// { +// case (int)PID_ID::PID_X: +// { +// success = m_client_pid_x.call(srv); +// } +// break; +// case (int)PID_ID::PID_Y: +// { +// success = m_client_pid_y.call(srv); +// } +// break; +// case (int)PID_ID::PID_Z: +// { +// success = m_client_pid_z.call(srv); +// } +// break; +// case (int)PID_ID::PID_ROLL: +// { +// success = m_client_pid_roll.call(srv); +// } +// break; +// case (int)PID_ID::PID_PITCH: +// { +// success = m_client_pid_pitch.call(srv); +// } +// break; +// case (int)PID_ID::PID_YAW: +// { +// success = m_client_pid_yaw.call(srv); +// } +// break; +// case (int)PID_ID::PID_YAW_RATE: +// { +// success = m_client_pid_yaw_rate.call(srv); +// } +// break; +// default: +// { +// std::cerr <<"Invalid Controller ID type" <(srv.request.controller_parameters.id)); +// return true; +// } +// else { +// ROS_ERROR("Failed to call service /update_controller/pid"); +// return false; +// } +// } +// else if(ID == MRFT) { +// MRFT_UpdateMsg* _update_msg = (MRFT_UpdateMsg*)t_msg; +// hear_msgs::Update_Controller_MRFT srv; +// srv.request.controller_parameters.id = static_cast(_update_msg->param.id); +// srv.request.controller_parameters.mrft_beta = _update_msg->param.beta; +// srv.request.controller_parameters.mrft_relay_amp = _update_msg->param.relay_amp; +// srv.request.controller_parameters.mrft_no_switch_delay = _update_msg->param.no_switch_delay_in_ms; +// srv.request.controller_parameters.mrft_conf_samples = _update_msg->param.num_of_peak_conf_samples; +// bool success = false; + +// switch((int)_update_msg->param.id) +// { +// case (int)MRFT_ID::MRFT_X: +// { +// success = m_client_mrft_x.call(srv); +// } +// break; +// case (int)MRFT_ID::MRFT_Y: +// { +// success = m_client_mrft_y.call(srv); +// } +// break; +// case (int)MRFT_ID::MRFT_Z: +// { +// success = m_client_mrft_z.call(srv); +// } +// break; +// case (int)MRFT_ID::MRFT_ROLL: +// { +// success = m_client_mrft_roll.call(srv); +// } +// break; +// case (int)MRFT_ID::MRFT_PITCH: +// { +// success = m_client_mrft_pitch.call(srv); +// } +// break; +// case (int)MRFT_ID::MRFT_YAW: +// { +// success = m_client_mrft_yaw.call(srv); +// } +// break; +// case (int)MRFT_ID::MRFT_YAW_RATE: +// { +// success = m_client_mrft_yaw_rate.call(srv); +// } +// break; +// default: +// { +// std::cerr <<"Invalid Controller ID type" <(srv.request.controller_parameters.id)); +// return true; +// } +// else { +// ROS_ERROR("Failed to call service /update_controller/mrft"); +// return false; +// } +// } +// else if(ID == BOUNDINGCTRL){ +// BoundingCtrl_UpdateMsg* _update_msg = (BoundingCtrl_UpdateMsg*)t_msg; +// hear_msgs::Update_Controller_Bounding srv; +// srv.request.controller_parameters.id = static_cast(_update_msg->param.id); +// srv.request.controller_parameters.bounding_eps_1 = _update_msg->param.eps_1; +// srv.request.controller_parameters.bounding_eps_2 = _update_msg->param.eps_2; +// srv.request.controller_parameters.bounding_h_o1 = _update_msg->param.h_o1; +// srv.request.controller_parameters.bounding_h_o2 = _update_msg->param.h_o2; +// bool success = false; + +// switch((int)_update_msg->param.id) +// { +// case (int)BOUNDING_ID::BOUNDING_X: +// { +// success = m_client_bounding_x.call(srv); +// } +// break; +// case (int)BOUNDING_ID::BOUNDING_Y: +// { +// success = m_client_bounding_y.call(srv); +// } +// break; +// case (int)BOUNDING_ID::BOUNDING_Z: +// { +// success = m_client_bounding_z.call(srv); +// } +// break; +// default: +// { +// std::cerr <<"Invalid Controller ID type" <(srv.request.controller_parameters.id)); +// return true; +// } +// else { +// ROS_ERROR("Failed to call service /update_controller/bounding"); +// return false; +// } +// } +// else{ +// ROS_ERROR("Failed to update controller"); +// return false; +// } +// } + +// } diff --git a/src/ROSUnit_UpdateContSrv.cpp b/src/ROSUnit_UpdateContSrv.cpp old mode 100644 new mode 100755 index ca34698..d4d5e1e --- a/src/ROSUnit_UpdateContSrv.cpp +++ b/src/ROSUnit_UpdateContSrv.cpp @@ -2,8 +2,8 @@ namespace HEAR{ -UpdateTrigger* ROSUnit_UpdateContSrv::registerServer(const std::string &service_topic){ - ext_trig = new UpdateTrigger; +ExternalTrigger* ROSUnit_UpdateContSrv::registerServer(const std::string &service_topic){ + ext_trig = new ExternalTrigger; this->m_server = this->nh_.advertiseService(service_topic, &ROSUnit_UpdateContSrv::srv_callback, this); return ext_trig; } @@ -17,7 +17,7 @@ bool ROSUnit_UpdateContSrv::srv_callback(hear_msgs::Update_Controller_PID::Reque msg.param.ki = req.controller_parameters.pid_ki; msg.param.kd = req.controller_parameters.pid_kd; msg.param.kdd = req.controller_parameters.pid_kdd; - ext_trig->UpdateCallback((UpdateMsg*)&msg); + ext_trig->UpdateCallback((BaseMsg*)&msg); return true; } diff --git a/src/ROSUnit_UpdateMRFTContClnt.cpp b/src/ROSUnit_UpdateMRFTContClnt.cpp new file mode 100755 index 0000000..4dbb9df --- /dev/null +++ b/src/ROSUnit_UpdateMRFTContClnt.cpp @@ -0,0 +1,38 @@ +#include "HEAR_ROS/ROSUnit_UpdateMRFTContClnt.hpp" + +namespace HEAR{ + +ROSUnit_UpdateMRFTContClnt::ROSUnit_UpdateMRFTContClnt(ros::NodeHandle& t_main_handler, std::string Drone_Name, std::string Channel) : nh_(t_main_handler){ + + m_client_mrft = nh_.serviceClient(Drone_Name + "update_controller/mrft/" + Channel); + +} + +ROSUnit_UpdateMRFTContClnt::~ROSUnit_UpdateMRFTContClnt() { + +} + +bool ROSUnit_UpdateMRFTContClnt::process(BaseMsg* t_msg) { + MRFT_UpdateMsg* _update_msg = (MRFT_UpdateMsg*)t_msg; + hear_msgs::Update_Controller_MRFT srv; + srv.request.controller_parameters.id = (int)_update_msg->param.id; + srv.request.controller_parameters.mrft_beta = _update_msg->param.beta; + srv.request.controller_parameters.mrft_relay_amp = _update_msg->param.relay_amp; + srv.request.controller_parameters.mrft_no_switch_delay = _update_msg->param.no_switch_delay_in_ms; + srv.request.controller_parameters.mrft_conf_samples = _update_msg->param.num_of_peak_conf_samples; + bool success = false; + + success = m_client_mrft.call(srv); + + if (success) { + ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + return true; + } + else { + ROS_ERROR("Failed to call service /update_controller/mrft"); + return false; + } + +} + +} diff --git a/src/ROSUnit_UpdateMRFTsrv.cpp b/src/ROSUnit_UpdateMRFTsrv.cpp old mode 100644 new mode 100755 index e55761d..df3a9fc --- a/src/ROSUnit_UpdateMRFTsrv.cpp +++ b/src/ROSUnit_UpdateMRFTsrv.cpp @@ -2,8 +2,8 @@ namespace HEAR{ -UpdateTrigger* ROSUnit_UpdateMRFTsrv::registerServer(const std::string &service_topic){ - ext_trig = new UpdateTrigger; +ExternalTrigger* ROSUnit_UpdateMRFTsrv::registerServer(const std::string &service_topic){ + ext_trig = new ExternalTrigger; this->m_server = this->nh_.advertiseService(service_topic, &ROSUnit_UpdateMRFTsrv::srv_callback, this); return ext_trig; } @@ -15,7 +15,7 @@ bool ROSUnit_UpdateMRFTsrv::srv_callback(hear_msgs::Update_Controller_MRFT::Requ msg.param.relay_amp = req.controller_parameters.mrft_relay_amp ; msg.param.no_switch_delay_in_ms = req.controller_parameters.mrft_no_switch_delay; msg.param.num_of_peak_conf_samples = req.controller_parameters.mrft_conf_samples; - ext_trig->UpdateCallback((UpdateMsg*)&msg); + ext_trig->UpdateCallback((BaseMsg*)&msg); return true; } diff --git a/src/ROSUnit_UpdatePIDContClnt.cpp b/src/ROSUnit_UpdatePIDContClnt.cpp new file mode 100755 index 0000000..26f315e --- /dev/null +++ b/src/ROSUnit_UpdatePIDContClnt.cpp @@ -0,0 +1,41 @@ +#include "HEAR_ROS/ROSUnit_UpdatePIDContClnt.hpp" + +namespace HEAR{ + +ROSUnit_UpdatePIDContClnt::ROSUnit_UpdatePIDContClnt(ros::NodeHandle& t_main_handler, std::string Drone_Name, std::string Channel) : nh_(t_main_handler){ + + m_client_pid = nh_.serviceClient(Drone_Name + "update_controller/pid/" + Channel); + +} + +ROSUnit_UpdatePIDContClnt::~ROSUnit_UpdatePIDContClnt() { + +} + +bool ROSUnit_UpdatePIDContClnt::process(BaseMsg* t_msg) { + PID_UpdateMsg* _update_msg = (PID_UpdateMsg*)t_msg; + hear_msgs::Update_Controller_PID srv; + srv.request.controller_parameters.id = (int)_update_msg->param.id; + srv.request.controller_parameters.pid_kp = _update_msg->param.kp; + srv.request.controller_parameters.pid_ki = _update_msg->param.ki; + srv.request.controller_parameters.pid_kd = _update_msg->param.kd; + srv.request.controller_parameters.pid_kdd = _update_msg->param.kdd; + srv.request.controller_parameters.pid_anti_windup = _update_msg->param.anti_windup; + srv.request.controller_parameters.pid_en_pv_derivation = _update_msg->param.en_pv_derivation; + srv.request.controller_parameters.pid_dt = _update_msg->param.dt; + bool success = false; + + success = m_client_pid.call(srv); + + if (success) { + ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + return true; + } + else { + ROS_ERROR("Failed to call service /update_controller/pid"); + return false; + } + +} + +} diff --git a/src/ROSUnit_UpdateTrajectoryClnt.cpp b/src/ROSUnit_UpdateTrajectoryClnt.cpp new file mode 100755 index 0000000..df93fff --- /dev/null +++ b/src/ROSUnit_UpdateTrajectoryClnt.cpp @@ -0,0 +1,42 @@ +#include "HEAR_ROS/ROSUnit_UpdateTrajectoryClnt.hpp" + +namespace HEAR{ + +ROSUnit_UpdateTrajectoryClnt::ROSUnit_UpdateTrajectoryClnt(ros::NodeHandle& t_main_handler, std::string Drone_Name) : nh_(t_main_handler){ + + m_client = nh_.serviceClient(Drone_Name + "generate_trajectory"); + + +} + +ROSUnit_UpdateTrajectoryClnt::~ROSUnit_UpdateTrajectoryClnt() { + +} + +bool ROSUnit_UpdateTrajectoryClnt::process(BaseMsg* t_msg) { + Trajectory_UpdateMsg* _update_msg = (Trajectory_UpdateMsg*)t_msg; + hear_msgs::Update_Trajectory srv; + srv.request.trajectory_parameters.trajectoryType = (int)_update_msg->param._trajectoryType; + srv.request.trajectory_parameters.samplingType = (int)_update_msg->param._samplingType; + srv.request.trajectory_parameters.transformationType = (int)_update_msg->param._transformationType; + srv.request.trajectory_parameters.scale = _update_msg->param.scale; + srv.request.trajectory_parameters.rot = _update_msg->param.rot; + srv.request.trajectory_parameters.trans = _update_msg->param.trans; + srv.request.trajectory_parameters.TotalExecutionTime = _update_msg->param.TotalExecutionTime; + srv.request.trajectory_parameters.Velocity = _update_msg->param.Velocity; + srv.request.trajectory_parameters.ClearQ = _update_msg->param.ClearQ; + bool success = false; + success = m_client.call(srv); + + if (success) { + ROS_INFO("TRAJECTORY UPDATED."); + return true; + } + else { + ROS_ERROR("Failed to call service /generate_trajectory"); + return false; + } + +} + +} diff --git a/src/ROSUnit_UpdateTrajectorySrv.cpp b/src/ROSUnit_UpdateTrajectorySrv.cpp new file mode 100755 index 0000000..74ab9c2 --- /dev/null +++ b/src/ROSUnit_UpdateTrajectorySrv.cpp @@ -0,0 +1,27 @@ +#include "HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp" + +namespace HEAR{ + +ExternalTrigger* ROSUnit_UpdateTrajectorySrv::registerServer(const std::string &service_topic){ + ext_trig = new ExternalTrigger; + this->m_server = this->nh_.advertiseService(service_topic, &ROSUnit_UpdateTrajectorySrv::srv_callback, this); + return ext_trig; +} + +bool ROSUnit_UpdateTrajectorySrv::srv_callback(hear_msgs::Update_Trajectory::Request& req, hear_msgs::Update_Trajectory::Response& res){ + Trajectory_UpdateMsg msg; + msg.param._trajectoryType = (int)req.trajectory_parameters.trajectoryType; + msg.param._samplingType = (int)req.trajectory_parameters.samplingType; + msg.param._transformationType = (int)req.trajectory_parameters.transformationType; + msg.param.rot = req.trajectory_parameters.rot; + msg.param.trans = req.trajectory_parameters.trans; + msg.param.scale = req.trajectory_parameters.scale; + msg.param.TotalExecutionTime = req.trajectory_parameters.TotalExecutionTime; + msg.param.Velocity = req.trajectory_parameters.Velocity; + msg.param.ClearQ = req.trajectory_parameters.ClearQ; + + ext_trig->UpdateCallback((BaseMsg*)&msg); + return true; +} + +} \ No newline at end of file