From b8bba2548b93b144da2578ac018514042aca0375 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Fri, 29 Jul 2022 16:40:56 +0400 Subject: [PATCH 01/36] Add Bool and Float Clients 25.7.2022 --- include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 21 +++++++++++++++++++++ include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 21 +++++++++++++++++++++ src/ROSUnit_BoolClnt.cpp | 15 +++++++++++++++ src/ROSUnit_FloatClnt.cpp | 15 +++++++++++++++ 4 files changed, 72 insertions(+) create mode 100644 include/HEAR_ROS/ROSUnit_BoolClnt.hpp create mode 100644 include/HEAR_ROS/ROSUnit_FloatClnt.hpp create mode 100644 src/ROSUnit_BoolClnt.cpp create mode 100644 src/ROSUnit_FloatClnt.cpp diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp new file mode 100644 index 0000000..d2a0e8c --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -0,0 +1,21 @@ +#ifndef ROSUNIT_BOOLCLNT +#define ROSUNIT_BOOLCLNT + +#include +#include +#include "HEAR_core/DataTypes.hpp" +#include + +namespace HEAR{ + +class ROSUnitBoolClient { +private: + ros::ServiceClient m_client; +public: + ROSUnitBoolClient(ros::NodeHandle&, std::string); + void process(DataMsg* t_msg); +}; + +} + +#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 100644 index 0000000..eb015c2 --- /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 + +namespace HEAR{ + +class ROSUnitFloatClient { +private: + ros::ServiceClient m_client; +public: + ROSUnitFloatClient(ros::NodeHandle&, std::string); + void process(DataMsg* t_msg); +}; + +} + +#endif \ No newline at end of file diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp new file mode 100644 index 0000000..9a80321 --- /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){ + m_client = nh.serviceClient(t_name); +} + +void ROSUnitBoolClient::process(DataMsg* t_msg){ + hear_msgs::set_bool t_srv; + t_srv.request.data = ((BoolMsg*) t_msg)->data; + m_client.call(msg); +} + +} \ No newline at end of file diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp new file mode 100644 index 0000000..f925b4b --- /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){ + m_client = nh.serviceClient(t_name); +} + +void ROSUnitFloatClient::process(DataMsg* t_msg){ + hear_msgs::set_float t_srv; + t_srv.request.data = ((FloatMsg*) t_msg)->data; + m_client.call(t_srv); +} + +} \ No newline at end of file From f08f2cc0d65433de0ec33547682a805c36238607 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Fri, 29 Jul 2022 16:41:21 +0400 Subject: [PATCH 02/36] Updated Bool and Float Clients --- include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 4 ++-- include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 2 +- src/ROSUnit_BoolClnt.cpp | 8 ++++---- src/ROSUnit_FloatClnt.cpp | 8 ++++---- 4 files changed, 11 insertions(+), 11 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp index d2a0e8c..5bca2d2 100644 --- a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -2,7 +2,7 @@ #define ROSUNIT_BOOLCLNT #include -#include +#include #include "HEAR_core/DataTypes.hpp" #include @@ -13,7 +13,7 @@ class ROSUnitBoolClient { ros::ServiceClient m_client; public: ROSUnitBoolClient(ros::NodeHandle&, std::string); - void process(DataMsg* t_msg); + bool process(bool data); }; } diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp index eb015c2..108becc 100644 --- a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp @@ -13,7 +13,7 @@ class ROSUnitFloatClient { ros::ServiceClient m_client; public: ROSUnitFloatClient(ros::NodeHandle&, std::string); - void process(DataMsg* t_msg); + float process(float data); }; } diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp index 9a80321..8867052 100644 --- a/src/ROSUnit_BoolClnt.cpp +++ b/src/ROSUnit_BoolClnt.cpp @@ -3,12 +3,12 @@ namespace HEAR{ ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name){ - m_client = nh.serviceClient(t_name); + m_client = nh.serviceClient(t_name); } -void ROSUnitBoolClient::process(DataMsg* t_msg){ - hear_msgs::set_bool t_srv; - t_srv.request.data = ((BoolMsg*) t_msg)->data; +bool ROSUnitBoolClient::process(bool data){ + std_srvs::SetBool msg; + msg.request.data = data; m_client.call(msg); } diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp index f925b4b..2b98ef3 100644 --- a/src/ROSUnit_FloatClnt.cpp +++ b/src/ROSUnit_FloatClnt.cpp @@ -6,10 +6,10 @@ ROSUnitFloatClient::ROSUnitFloatClient(ros::NodeHandle &nh, std::string t_name){ m_client = nh.serviceClient(t_name); } -void ROSUnitFloatClient::process(DataMsg* t_msg){ - hear_msgs::set_float t_srv; - t_srv.request.data = ((FloatMsg*) t_msg)->data; - m_client.call(t_srv); +float ROSUnitFloatClient::process(float data){ + hear_msgs::set_float msg; + msg.request.data = data; + m_client.call(msg); } } \ No newline at end of file From 86f4f74dd13eca3db8470549144d052484e7c150 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Mon, 22 Aug 2022 18:43:06 +0400 Subject: [PATCH 03/36] return statement added --- include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 2 +- src/ROSUnit_BoolClnt.cpp | 2 +- src/ROSUnit_FloatClnt.cpp | 4 ++-- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp index 108becc..249c194 100644 --- a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp @@ -13,7 +13,7 @@ class ROSUnitFloatClient { ros::ServiceClient m_client; public: ROSUnitFloatClient(ros::NodeHandle&, std::string); - float process(float data); + bool process(float data); }; } diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp index 8867052..e62a8d6 100644 --- a/src/ROSUnit_BoolClnt.cpp +++ b/src/ROSUnit_BoolClnt.cpp @@ -9,7 +9,7 @@ ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name){ bool ROSUnitBoolClient::process(bool data){ std_srvs::SetBool msg; msg.request.data = data; - m_client.call(msg); + return m_client.call(msg); } } \ No newline at end of file diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp index 2b98ef3..5432744 100644 --- a/src/ROSUnit_FloatClnt.cpp +++ b/src/ROSUnit_FloatClnt.cpp @@ -6,10 +6,10 @@ ROSUnitFloatClient::ROSUnitFloatClient(ros::NodeHandle &nh, std::string t_name){ m_client = nh.serviceClient(t_name); } -float ROSUnitFloatClient::process(float data){ +bool ROSUnitFloatClient::process(float data){ hear_msgs::set_float msg; msg.request.data = data; - m_client.call(msg); + return m_client.call(msg); } } \ No newline at end of file From efdf51abeb458c3382ba8540a0081075f7efe010 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 23 Aug 2022 17:29:13 +0400 Subject: [PATCH 04/36] Add ROSUnit_Client.hpp --- include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 6 +++--- include/HEAR_ROS/ROSUnit_Client.hpp | 15 +++++++++++++++ include/HEAR_ROS/ROSUnit_EmptyClnt.hpp | 5 ++--- include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 5 ++--- 4 files changed, 22 insertions(+), 9 deletions(-) create mode 100644 include/HEAR_ROS/ROSUnit_Client.hpp diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp index 5bca2d2..355d05f 100644 --- a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -4,13 +4,13 @@ #include #include #include "HEAR_core/DataTypes.hpp" +#include "HEAR_ROS/ROSUnit_Client.hpp" #include namespace HEAR{ -class ROSUnitBoolClient { -private: - ros::ServiceClient m_client; +class ROSUnitBoolClient: public ROSUnit_Client{ + public: ROSUnitBoolClient(ros::NodeHandle&, std::string); bool process(bool data); diff --git a/include/HEAR_ROS/ROSUnit_Client.hpp b/include/HEAR_ROS/ROSUnit_Client.hpp new file mode 100644 index 0000000..6208732 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_Client.hpp @@ -0,0 +1,15 @@ +#ifndef ROSUNIT_CLIENT_HPP +#define ROSUNIT_CLIENT_HPP + +#include +#include "HEAR_core/DataTypes.hpp" + +namespace HEAR{ + +class ROSUnit_Client { +protected: + ros::ServiceClient m_client; +}; +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp index 63628db..24f498b 100644 --- a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp @@ -3,12 +3,11 @@ #include #include +#include "HEAR_ROS/ROSUnit_Client.hpp" namespace HEAR{ -class ROSUnitEmptyClient { -private: - ros::ServiceClient m_client; +class ROSUnitEmptyClient: public ROSUnit_Client{ public: ROSUnitEmptyClient(ros::NodeHandle&, std::string); bool process(); diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp index 249c194..6c6a043 100644 --- a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp @@ -4,13 +4,12 @@ #include #include #include "HEAR_core/DataTypes.hpp" +#include "HEAR_ROS/ROSUnit_Client.hpp" #include namespace HEAR{ -class ROSUnitFloatClient { -private: - ros::ServiceClient m_client; +class ROSUnitFloatClient : public ROSUnit_Client{ public: ROSUnitFloatClient(ros::NodeHandle&, std::string); bool process(float data); From 9a2a9214c86a07127a6ab38aa0012a99cefc62d2 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 31 Aug 2022 16:12:31 +0400 Subject: [PATCH 05/36] UpdateControllerClnt added --- hear_ros.cmake | 11 +++ include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 41 ++++++++ src/ROSUnit_UpdateContClnt.cpp | 102 ++++++++++++++++++++ 3 files changed, 154 insertions(+) create mode 100644 include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp create mode 100644 src/ROSUnit_UpdateContClnt.cpp diff --git a/hear_ros.cmake b/hear_ros.cmake index b58d851..c103006 100644 --- a/hear_ros.cmake +++ b/hear_ros.cmake @@ -2,3 +2,14 @@ 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) + +find_package(catkin REQUIRED COMPONENTS + roscpp + hear_msgs + geometry_msgs + tf2_geometry_msgs + tf2 + tf2_ros +) + +set(HEAR_ROS_LIBS ${catkin_LIBRARIES}) \ 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 100644 index 0000000..7d76fba --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -0,0 +1,41 @@ +#ifndef ROSUNIT_UPDATECONTCLNT_HPP +#define ROSUNIT_UPDATECONTCLNT_HPP + + +#include +#include +#include +#include +#include +#include "HEAR_core/DataTypes.hpp" + + + +namespace HEAR{ +class ROSUnit_UpdateContClnt { +private: + UpdateMsg _update_controller_msg; + 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; +public: + void process(); + ROSUnit_UpdateContClnt(ros::NodeHandle&); + ~ROSUnit_UpdateContClnt(); + +}; + +} + +#endif \ No newline at end of file diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp new file mode 100644 index 0000000..43fcfb1 --- /dev/null +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -0,0 +1,102 @@ +#include "HEAR_ROS/ROSUnit_UpdateContClnt.hpp" + +namespace HEAR{ + +ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler) { + + // PID of the inner loop + m_client_pid_x = t_main_handler.serviceClient("update_controller/pid/x"); + m_client_pid_y = t_main_handler.serviceClient("update_controller/pid/y"); + m_client_pid_z = t_main_handler.serviceClient("update_controller/pid/z"); + + // PID of the outer loop + m_client_pid_roll = t_main_handler.serviceClient("update_controller/pid/roll"); + m_client_pid_pitch = t_main_handler.serviceClient("update_controller/pid/pitch"); + m_client_pid_yaw = t_main_handler.serviceClient("update_controller/pid/yaw"); + m_client_pid_yaw_rate = t_main_handler.serviceClient("update_controller/pid/yaw_rate"); + + // MRFT of the inner loop + m_client_mrft_x = t_main_handler.serviceClient("update_controller/mrft/x"); + m_client_mrft_y = t_main_handler.serviceClient("update_controller/mrft/y"); + m_client_mrft_z = t_main_handler.serviceClient("update_controller/mrft/z"); + + // MRFT of the outer loop + m_client_mrft_roll = t_main_handler.serviceClient("update_controller/mrft/roll"); + m_client_mrft_pitch = t_main_handler.serviceClient("update_controller/mrft/pitch"); + m_client_mrft_yaw = t_main_handler.serviceClient("update_controller/mrft/yaw"); + m_client_mrft_yaw_rate = t_main_handler.serviceClient("update_controller/mrft/yaw_rate"); +} + +ROSUnit_UpdateContClnt::~ROSUnit_UpdateContClnt() { + +} + +void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, CONTROLSYS_ID ID) { + //TODO: Write this function + 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 success1 = false; + bool success2 = false; + bool success3 = false; + bool success4 = false; + if( (int)_update_msg->param.id <= (int)PID_ID::PID_Z){ + success1 = m_client_pid_x.call(srv); + success2 = m_client_pid_y.call(srv); + success3 = m_client_pid_z.call(srv); + success4 = true; + }else{ + success1 = m_client_pid_roll.call(srv); + success2 = m_client_pid_pitch.call(srv); + success3 = m_client_pid_yaw.call(srv); + success4 = m_client_pid_yaw_rate.call(srv); + } + if (success1 && success2 && success3 && success4) { + ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + } + else { + ROS_ERROR("Failed to call service /update_controller"); + } + } + 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_bias = _update_msg->param.bias; + 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 success1 = false; + bool success2 = false; + bool success3 = false; + bool success4 = false; + if( (int)_update_msg->param.id <= (int)MRFT_ID::MRFT_Z){ + success1 = m_client_mrft_x.call(srv); + success2 = m_client_mrft_y.call(srv); + success3 = m_client_mrft_z.call(srv); + success4 = true; + }else{ + success1 = m_client_mrft_roll.call(srv); + success2 = m_client_mrft_pitch.call(srv); + success3 = m_client_mrft_yaw.call(srv); + success4 = m_client_mrft_yaw_rate.call(srv); + } + if (success1 && success2 && success3 && success4) { + ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + } + else { + ROS_ERROR("Failed to call service /update_controller"); + } + } +} + +} \ No newline at end of file From 05d207d33b3d75f845963c2198e73dc5b1f20c0d Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Thu, 1 Sep 2022 18:58:58 +0400 Subject: [PATCH 06/36] Fixing NodeHandle calling --- include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 3 ++- include/HEAR_ROS/ROSUnit_EmptyClnt.hpp | 1 + include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 1 + include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 1 + src/ROSUnit_BoolClnt.cpp | 4 +-- src/ROSUnit_EmptyClnt.cpp | 4 +-- src/ROSUnit_FloatClnt.cpp | 4 +-- src/ROSUnit_UpdateContClnt.cpp | 30 ++++++++++----------- 8 files changed, 26 insertions(+), 22 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp index 355d05f..d114043 100644 --- a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -12,7 +12,8 @@ namespace HEAR{ class ROSUnitBoolClient: public ROSUnit_Client{ public: - ROSUnitBoolClient(ros::NodeHandle&, std::string); + ros::NodeHandle nh_; + ROSUnitBoolClient(ros::NodeHandle& nh, std::string); bool process(bool data); }; diff --git a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp index 24f498b..6816249 100644 --- a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp @@ -9,6 +9,7 @@ namespace HEAR{ class ROSUnitEmptyClient: public ROSUnit_Client{ public: + ros::NodeHandle nh_; ROSUnitEmptyClient(ros::NodeHandle&, std::string); bool process(); }; diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp index 6c6a043..150d1bf 100644 --- a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp @@ -11,6 +11,7 @@ namespace HEAR{ class ROSUnitFloatClient : public ROSUnit_Client{ public: + ros::NodeHandle nh_; ROSUnitFloatClient(ros::NodeHandle&, std::string); bool process(float data); }; diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp index 7d76fba..8ed033b 100644 --- a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -14,6 +14,7 @@ namespace HEAR{ class ROSUnit_UpdateContClnt { private: + ros::NodeHandle nh_; UpdateMsg _update_controller_msg; ros::ServiceClient m_client_pid_x; ros::ServiceClient m_client_pid_y; diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp index e62a8d6..cd85151 100644 --- a/src/ROSUnit_BoolClnt.cpp +++ b/src/ROSUnit_BoolClnt.cpp @@ -2,8 +2,8 @@ namespace HEAR{ -ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name){ - m_client = nh.serviceClient(t_name); +ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name): nh_(nh){ + m_client = nh_.serviceClient(t_name); } bool ROSUnitBoolClient::process(bool data){ diff --git a/src/ROSUnit_EmptyClnt.cpp b/src/ROSUnit_EmptyClnt.cpp index 7154016..e4a57f7 100644 --- a/src/ROSUnit_EmptyClnt.cpp +++ b/src/ROSUnit_EmptyClnt.cpp @@ -2,8 +2,8 @@ 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(){ diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp index 5432744..274765d 100644 --- a/src/ROSUnit_FloatClnt.cpp +++ b/src/ROSUnit_FloatClnt.cpp @@ -2,8 +2,8 @@ namespace HEAR{ -ROSUnitFloatClient::ROSUnitFloatClient(ros::NodeHandle &nh, std::string t_name){ - m_client = nh.serviceClient(t_name); +ROSUnitFloatClient::ROSUnitFloatClient(ros::NodeHandle &nh, std::string t_name) : nh_(nh){ + m_client = nh_.serviceClient(t_name); } bool ROSUnitFloatClient::process(float data){ diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index 43fcfb1..15c4117 100644 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -2,29 +2,29 @@ namespace HEAR{ -ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler) { +ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler) : nh_(t_main_handler){ // PID of the inner loop - m_client_pid_x = t_main_handler.serviceClient("update_controller/pid/x"); - m_client_pid_y = t_main_handler.serviceClient("update_controller/pid/y"); - m_client_pid_z = t_main_handler.serviceClient("update_controller/pid/z"); + m_client_pid_x = nh_.serviceClient("update_controller/pid/x"); + m_client_pid_y = nh_.serviceClient("update_controller/pid/y"); + m_client_pid_z = nh_.serviceClient("update_controller/pid/z"); // PID of the outer loop - m_client_pid_roll = t_main_handler.serviceClient("update_controller/pid/roll"); - m_client_pid_pitch = t_main_handler.serviceClient("update_controller/pid/pitch"); - m_client_pid_yaw = t_main_handler.serviceClient("update_controller/pid/yaw"); - m_client_pid_yaw_rate = t_main_handler.serviceClient("update_controller/pid/yaw_rate"); + m_client_pid_roll = nh_.serviceClient("update_controller/pid/roll"); + m_client_pid_pitch = nh_.serviceClient("update_controller/pid/pitch"); + m_client_pid_yaw = nh_.serviceClient("update_controller/pid/yaw"); + m_client_pid_yaw_rate = nh_.serviceClient("update_controller/pid/yaw_rate"); // MRFT of the inner loop - m_client_mrft_x = t_main_handler.serviceClient("update_controller/mrft/x"); - m_client_mrft_y = t_main_handler.serviceClient("update_controller/mrft/y"); - m_client_mrft_z = t_main_handler.serviceClient("update_controller/mrft/z"); + m_client_mrft_x = nh_.serviceClient("update_controller/mrft/x"); + m_client_mrft_y = nh_.serviceClient("update_controller/mrft/y"); + m_client_mrft_z = nh_.serviceClient("update_controller/mrft/z"); // MRFT of the outer loop - m_client_mrft_roll = t_main_handler.serviceClient("update_controller/mrft/roll"); - m_client_mrft_pitch = t_main_handler.serviceClient("update_controller/mrft/pitch"); - m_client_mrft_yaw = t_main_handler.serviceClient("update_controller/mrft/yaw"); - m_client_mrft_yaw_rate = t_main_handler.serviceClient("update_controller/mrft/yaw_rate"); + m_client_mrft_roll = nh_.serviceClient("update_controller/mrft/roll"); + m_client_mrft_pitch = nh_.serviceClient("update_controller/mrft/pitch"); + m_client_mrft_yaw = nh_.serviceClient("update_controller/mrft/yaw"); + m_client_mrft_yaw_rate = nh_.serviceClient("update_controller/mrft/yaw_rate"); } ROSUnit_UpdateContClnt::~ROSUnit_UpdateContClnt() { From 29b26e3e8ddc36381fab05fb5e35de19c400e163 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Mon, 5 Sep 2022 17:31:52 +0400 Subject: [PATCH 07/36] Build Trial 1 --- include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 2 +- src/ROSUnit_UpdateContClnt.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp index 8ed033b..c66550c 100644 --- a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -31,7 +31,7 @@ class ROSUnit_UpdateContClnt { ros::ServiceClient m_client_mrft_yaw; ros::ServiceClient m_client_mrft_yaw_rate; public: - void process(); + void process(UpdateMsg* t_msg, BLOCK_ID ID); ROSUnit_UpdateContClnt(ros::NodeHandle&); ~ROSUnit_UpdateContClnt(); diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index 15c4117..af4c4ba 100644 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -31,7 +31,7 @@ ROSUnit_UpdateContClnt::~ROSUnit_UpdateContClnt() { } -void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, CONTROLSYS_ID ID) { +void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { //TODO: Write this function if(ID == PID) { PID_UpdateMsg* _update_msg = (PID_UpdateMsg*)t_msg; From 286ab860512584d869926dfa24c6cf52499238f1 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 20 Sep 2022 11:45:14 +0400 Subject: [PATCH 08/36] Update HEAR_ROS --- include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 4 ++-- include/HEAR_ROS/ROSUnit_Client.hpp | 5 ++++- include/HEAR_ROS/ROSUnit_EmptyClnt.hpp | 2 +- include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 4 ++-- include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 3 +-- src/ROSUnit_BoolClnt.cpp | 4 ++-- src/ROSUnit_FloatClnt.cpp | 4 ++-- src/ROSUnit_UpdateContClnt.cpp | 12 +++++++++--- 8 files changed, 23 insertions(+), 15 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp index d114043..d8700cc 100644 --- a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -9,12 +9,12 @@ namespace HEAR{ -class ROSUnitBoolClient: public ROSUnit_Client{ +class ROSUnitBoolClient: public ROSUnit_Client{ public: ros::NodeHandle nh_; ROSUnitBoolClient(ros::NodeHandle& nh, std::string); - bool process(bool data); + bool process(); }; } diff --git a/include/HEAR_ROS/ROSUnit_Client.hpp b/include/HEAR_ROS/ROSUnit_Client.hpp index 6208732..120cc60 100644 --- a/include/HEAR_ROS/ROSUnit_Client.hpp +++ b/include/HEAR_ROS/ROSUnit_Client.hpp @@ -6,9 +6,12 @@ namespace HEAR{ +template class ROSUnit_Client { -protected: +public: + T data; ros::ServiceClient m_client; + virtual bool process() {return true;}; }; } diff --git a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp index 6816249..f8eed03 100644 --- a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp @@ -7,7 +7,7 @@ namespace HEAR{ -class ROSUnitEmptyClient: public ROSUnit_Client{ +class ROSUnitEmptyClient: public ROSUnit_Client{ public: ros::NodeHandle nh_; ROSUnitEmptyClient(ros::NodeHandle&, std::string); diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp index 150d1bf..655269c 100644 --- a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp @@ -9,11 +9,11 @@ namespace HEAR{ -class ROSUnitFloatClient : public ROSUnit_Client{ +class ROSUnitFloatClient : public ROSUnit_Client{ public: ros::NodeHandle nh_; ROSUnitFloatClient(ros::NodeHandle&, std::string); - bool process(float data); + bool process(); }; } diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp index c66550c..3db8a7c 100644 --- a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -15,7 +15,6 @@ namespace HEAR{ class ROSUnit_UpdateContClnt { private: ros::NodeHandle nh_; - UpdateMsg _update_controller_msg; ros::ServiceClient m_client_pid_x; ros::ServiceClient m_client_pid_y; ros::ServiceClient m_client_pid_z; @@ -31,7 +30,7 @@ class ROSUnit_UpdateContClnt { ros::ServiceClient m_client_mrft_yaw; ros::ServiceClient m_client_mrft_yaw_rate; public: - void process(UpdateMsg* t_msg, BLOCK_ID ID); + bool process(UpdateMsg* t_msg, BLOCK_ID ID); ROSUnit_UpdateContClnt(ros::NodeHandle&); ~ROSUnit_UpdateContClnt(); diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp index cd85151..fbfa150 100644 --- a/src/ROSUnit_BoolClnt.cpp +++ b/src/ROSUnit_BoolClnt.cpp @@ -6,9 +6,9 @@ ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name): n m_client = nh_.serviceClient(t_name); } -bool ROSUnitBoolClient::process(bool data){ +bool ROSUnitBoolClient::process(){ std_srvs::SetBool msg; - msg.request.data = data; + msg.request.data = this->data; return m_client.call(msg); } diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp index 274765d..7ae5a26 100644 --- a/src/ROSUnit_FloatClnt.cpp +++ b/src/ROSUnit_FloatClnt.cpp @@ -6,9 +6,9 @@ ROSUnitFloatClient::ROSUnitFloatClient(ros::NodeHandle &nh, std::string t_name) m_client = nh_.serviceClient(t_name); } -bool ROSUnitFloatClient::process(float data){ +bool ROSUnitFloatClient::process(){ hear_msgs::set_float msg; - msg.request.data = data; + msg.request.data = this->data; return m_client.call(msg); } diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index af4c4ba..d66fa6c 100644 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -31,8 +31,7 @@ ROSUnit_UpdateContClnt::~ROSUnit_UpdateContClnt() { } -void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { - //TODO: Write this function +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; @@ -61,9 +60,11 @@ void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { } if (success1 && success2 && success3 && success4) { ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + return true; } else { ROS_ERROR("Failed to call service /update_controller"); + return false; } } else if(ID == MRFT) { @@ -72,7 +73,6 @@ void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { 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_bias = _update_msg->param.bias; 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 success1 = false; @@ -92,11 +92,17 @@ void ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { } if (success1 && success2 && success3 && success4) { ROS_INFO("CONTROLLER UPDATED. id: %d", static_cast(srv.request.controller_parameters.id)); + return true; } else { ROS_ERROR("Failed to call service /update_controller"); + return false; } } + else{ + ROS_ERROR("Failed to update controller"); + return false; + } } } \ No newline at end of file From 47ddfb44049b6d6507bf233a7ca0bda477e01642 Mon Sep 17 00:00:00 2001 From: Hazem Elrefaei <87086129+HazemElrefaei@users.noreply.github.com> Date: Wed, 21 Sep 2022 10:53:24 +0400 Subject: [PATCH 09/36] Update ROSUnit_UpdateContClnt.cpp main repository --- src/ROSUnit_UpdateContClnt.cpp | 125 +++++++++++++++++++++++++-------- 1 file changed, 94 insertions(+), 31 deletions(-) diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index d66fa6c..36eb3a3 100644 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -43,22 +43,53 @@ bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { 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 success1 = false; - bool success2 = false; - bool success3 = false; - bool success4 = false; - if( (int)_update_msg->param.id <= (int)PID_ID::PID_Z){ - success1 = m_client_pid_x.call(srv); - success2 = m_client_pid_y.call(srv); - success3 = m_client_pid_z.call(srv); - success4 = true; - }else{ - success1 = m_client_pid_roll.call(srv); - success2 = m_client_pid_pitch.call(srv); - success3 = m_client_pid_yaw.call(srv); - success4 = m_client_pid_yaw_rate.call(srv); + 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; } @@ -75,22 +106,54 @@ bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { 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 success1 = false; - bool success2 = false; - bool success3 = false; - bool success4 = false; - if( (int)_update_msg->param.id <= (int)MRFT_ID::MRFT_Z){ - success1 = m_client_mrft_x.call(srv); - success2 = m_client_mrft_y.call(srv); - success3 = m_client_mrft_z.call(srv); - success4 = true; - }else{ - success1 = m_client_mrft_roll.call(srv); - success2 = m_client_mrft_pitch.call(srv); - success3 = m_client_mrft_yaw.call(srv); - success4 = m_client_mrft_yaw_rate.call(srv); + 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; } @@ -105,4 +168,4 @@ bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { } } -} \ No newline at end of file +} From 05e6ca4f2725f6ed8b03b2cdb27f540a8c610ead Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Mon, 26 Sep 2022 12:36:38 +0400 Subject: [PATCH 10/36] Update HEAR_ROS --- include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 1 + include/HEAR_ROS/ROSUnit_BoolSrv.hpp | 0 include/HEAR_ROS/ROSUnit_Client.hpp | 0 include/HEAR_ROS/ROSUnit_EmptyClnt.hpp | 0 include/HEAR_ROS/ROSUnit_FloatArrPub.hpp | 0 include/HEAR_ROS/ROSUnit_FloatClnt.hpp | 0 include/HEAR_ROS/ROSUnit_FloatPub.hpp | 0 include/HEAR_ROS/ROSUnit_FloatSub.hpp | 0 include/HEAR_ROS/ROSUnit_Heartbeat.hpp | 0 include/HEAR_ROS/ROSUnit_PointPub.hpp | 0 include/HEAR_ROS/ROSUnit_PointSub.hpp | 0 include/HEAR_ROS/ROSUnit_PoseProvider.hpp | 0 include/HEAR_ROS/ROSUnit_Pub.hpp | 0 include/HEAR_ROS/ROSUnit_QuatPub.hpp | 0 include/HEAR_ROS/ROSUnit_QuatSub.hpp | 0 include/HEAR_ROS/ROSUnit_ResetSrv.hpp | 0 include/HEAR_ROS/ROSUnit_SLAM.hpp | 0 include/HEAR_ROS/ROSUnit_Sub.hpp | 0 include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 0 include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp | 0 include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp | 0 include/HEAR_ROS/RosSystem.hpp | 0 include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp | 0 src/ROSUnit_BoolClnt.cpp | 4 ++-- src/ROSUnit_BoolSrv.cpp | 0 src/ROSUnit_EmptyClnt.cpp | 0 src/ROSUnit_FloatClnt.cpp | 0 src/ROSUnit_PoseProvider.cpp | 0 src/ROSUnit_ResetSrv.cpp | 0 src/ROSUnit_SLAM.cpp | 0 src/ROSUnit_UpdateContClnt.cpp | 0 src/ROSUnit_UpdateContSrv.cpp | 0 src/ROSUnit_UpdateMRFTsrv.cpp | 0 33 files changed, 3 insertions(+), 2 deletions(-) mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_BoolClnt.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_BoolSrv.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_Client.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_EmptyClnt.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_FloatArrPub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_FloatClnt.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_FloatPub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_FloatSub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_Heartbeat.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_PointPub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_PointSub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_PoseProvider.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_Pub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_QuatPub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_QuatSub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_ResetSrv.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_SLAM.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_Sub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp mode change 100644 => 100755 include/HEAR_ROS/RosSystem.hpp mode change 100644 => 100755 include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp mode change 100644 => 100755 src/ROSUnit_BoolClnt.cpp mode change 100644 => 100755 src/ROSUnit_BoolSrv.cpp mode change 100644 => 100755 src/ROSUnit_EmptyClnt.cpp mode change 100644 => 100755 src/ROSUnit_FloatClnt.cpp mode change 100644 => 100755 src/ROSUnit_PoseProvider.cpp mode change 100644 => 100755 src/ROSUnit_ResetSrv.cpp mode change 100644 => 100755 src/ROSUnit_SLAM.cpp mode change 100644 => 100755 src/ROSUnit_UpdateContClnt.cpp mode change 100644 => 100755 src/ROSUnit_UpdateContSrv.cpp mode change 100644 => 100755 src/ROSUnit_UpdateMRFTsrv.cpp diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp old mode 100644 new mode 100755 index d8700cc..d2d3597 --- a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -2,6 +2,7 @@ #define ROSUNIT_BOOLCLNT #include +#include #include #include "HEAR_core/DataTypes.hpp" #include "HEAR_ROS/ROSUnit_Client.hpp" diff --git a/include/HEAR_ROS/ROSUnit_BoolSrv.hpp b/include/HEAR_ROS/ROSUnit_BoolSrv.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_Client.hpp b/include/HEAR_ROS/ROSUnit_Client.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_FloatClnt.hpp b/include/HEAR_ROS/ROSUnit_FloatClnt.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_FloatPub.hpp b/include/HEAR_ROS/ROSUnit_FloatPub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_FloatSub.hpp b/include/HEAR_ROS/ROSUnit_FloatSub.hpp old mode 100644 new mode 100755 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_PointPub.hpp b/include/HEAR_ROS/ROSUnit_PointPub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_PointSub.hpp b/include/HEAR_ROS/ROSUnit_PointSub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp old mode 100644 new mode 100755 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 diff --git a/include/HEAR_ROS/ROSUnit_QuatSub.hpp b/include/HEAR_ROS/ROSUnit_QuatSub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_ResetSrv.hpp b/include/HEAR_ROS/ROSUnit_ResetSrv.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_SLAM.hpp b/include/HEAR_ROS/ROSUnit_SLAM.hpp old mode 100644 new mode 100755 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_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/RosSystem.hpp b/include/HEAR_ROS/RosSystem.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp b/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_BoolClnt.cpp b/src/ROSUnit_BoolClnt.cpp old mode 100644 new mode 100755 index fbfa150..02daca0 --- a/src/ROSUnit_BoolClnt.cpp +++ b/src/ROSUnit_BoolClnt.cpp @@ -3,11 +3,11 @@ namespace HEAR{ ROSUnitBoolClient::ROSUnitBoolClient(ros::NodeHandle &nh, std::string t_name): nh_(nh){ - m_client = nh_.serviceClient(t_name); + m_client = nh_.serviceClient(t_name); } bool ROSUnitBoolClient::process(){ - std_srvs::SetBool msg; + hear_msgs::set_bool msg; msg.request.data = this->data; return m_client.call(msg); } diff --git a/src/ROSUnit_BoolSrv.cpp b/src/ROSUnit_BoolSrv.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_EmptyClnt.cpp b/src/ROSUnit_EmptyClnt.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_FloatClnt.cpp b/src/ROSUnit_FloatClnt.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_ResetSrv.cpp b/src/ROSUnit_ResetSrv.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_SLAM.cpp b/src/ROSUnit_SLAM.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_UpdateContSrv.cpp b/src/ROSUnit_UpdateContSrv.cpp old mode 100644 new mode 100755 diff --git a/src/ROSUnit_UpdateMRFTsrv.cpp b/src/ROSUnit_UpdateMRFTsrv.cpp old mode 100644 new mode 100755 From 69cc918d0d23466f600d9d8cd8f6017924905994 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 27 Sep 2022 17:00:15 +0400 Subject: [PATCH 11/36] Add Float server --- include/HEAR_ROS/ROSUnit_FloatSrv.hpp | 25 +++++++++++++++++++++++++ include/HEAR_ROS/RosSystem.hpp | 6 ++++++ src/ROSUnit_FloatSrv.cpp | 21 +++++++++++++++++++++ 3 files changed, 52 insertions(+) create mode 100644 include/HEAR_ROS/ROSUnit_FloatSrv.hpp create mode 100644 src/ROSUnit_FloatSrv.cpp diff --git a/include/HEAR_ROS/ROSUnit_FloatSrv.hpp b/include/HEAR_ROS/ROSUnit_FloatSrv.hpp new file mode 100644 index 0000000..b367820 --- /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; + UpdateTrigger* ext_trig; + bool srv_callback(hear_msgs::set_float::Request&, hear_msgs::set_float::Response&); +public: + ROSUnit_FloatServer(ros::NodeHandle&); + UpdateTrigger* 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 index 1808d7b..627d19a 100755 --- a/include/HEAR_ROS/RosSystem.hpp +++ b/include/HEAR_ROS/RosSystem.hpp @@ -18,6 +18,7 @@ #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 namespace HEAR{ @@ -178,6 +179,11 @@ ExternalTrigger* RosSystem::createUpdateTrigger(UPDATE_MSG_TYPE type, std::strin trig = srv->registerServer(topic); break; } + case UPDATE_MSG_TYPE::CONSTANT_UPDATE : { + auto srv = new ROSUnit_FloatServer(pnh_); + trig = srv->registerServer(topic); + break; + } default: return NULL; } diff --git a/src/ROSUnit_FloatSrv.cpp b/src/ROSUnit_FloatSrv.cpp new file mode 100644 index 0000000..ee5002b --- /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 UpdateTrigger; +} + +UpdateTrigger* 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){ + Variable_UpdateMsg msg; + msg._value = req.data; + ext_trig->UpdateCallback((UpdateMsg*)&msg); + return true; +} + +} \ No newline at end of file From 2e02c3c93be45748abfe70ca792628ce5f29f678 Mon Sep 17 00:00:00 2001 From: Hazem Elrefaei <87086129+HazemElrefaei@users.noreply.github.com> Date: Thu, 6 Oct 2022 16:18:31 +0400 Subject: [PATCH 12/36] Update ROSUnit_UpdateContClnt.cpp --- src/ROSUnit_UpdateContClnt.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index 36eb3a3..0634995 100755 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -4,23 +4,23 @@ namespace HEAR{ ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler) : nh_(t_main_handler){ - // PID of the inner loop + // PID of the outer loop m_client_pid_x = nh_.serviceClient("update_controller/pid/x"); m_client_pid_y = nh_.serviceClient("update_controller/pid/y"); m_client_pid_z = nh_.serviceClient("update_controller/pid/z"); - // PID of the outer loop + // PID of the inner loop m_client_pid_roll = nh_.serviceClient("update_controller/pid/roll"); m_client_pid_pitch = nh_.serviceClient("update_controller/pid/pitch"); m_client_pid_yaw = nh_.serviceClient("update_controller/pid/yaw"); m_client_pid_yaw_rate = nh_.serviceClient("update_controller/pid/yaw_rate"); - // MRFT of the inner loop + // MRFT of the outer loop m_client_mrft_x = nh_.serviceClient("update_controller/mrft/x"); m_client_mrft_y = nh_.serviceClient("update_controller/mrft/y"); m_client_mrft_z = nh_.serviceClient("update_controller/mrft/z"); - // MRFT of the outer loop + // MRFT of the inner loop m_client_mrft_roll = nh_.serviceClient("update_controller/mrft/roll"); m_client_mrft_pitch = nh_.serviceClient("update_controller/mrft/pitch"); m_client_mrft_yaw = nh_.serviceClient("update_controller/mrft/yaw"); From f4a9a43118f6e80c4649888604e6a4ad3728aa37 Mon Sep 17 00:00:00 2001 From: Hazem Elrefaei <87086129+HazemElrefaei@users.noreply.github.com> Date: Mon, 10 Oct 2022 13:07:54 +0400 Subject: [PATCH 13/36] Update RosUnit_MRFTSwitchSrv.hpp --- include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp b/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp index 59ac3dd..d1dcc14 100755 --- a/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp +++ b/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp @@ -73,8 +73,9 @@ bool ROSUnit_MRFTSwitchSrv::srvCallback(hear_msgs::set_float::Request& req, hear trig.first->UpdateCallback((UpdateMsg*)&sw_msg); } } + return true; } } -#endif \ No newline at end of file +#endif From 41a28c158a209ce390298bfa44aea709f66eec96 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 8 Nov 2022 11:05:06 +0400 Subject: [PATCH 14/36] Multiagent and new trajectory --- include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 3 +- src/ROSUnit_PoseProvider.cpp | 2 +- src/ROSUnit_UpdateContClnt.cpp | 32 ++++++++++----------- 3 files changed, 19 insertions(+), 18 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp index 3db8a7c..bc1f014 100755 --- a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -8,6 +8,7 @@ #include #include #include "HEAR_core/DataTypes.hpp" +#include @@ -31,7 +32,7 @@ class ROSUnit_UpdateContClnt { ros::ServiceClient m_client_mrft_yaw_rate; public: bool process(UpdateMsg* t_msg, BLOCK_ID ID); - ROSUnit_UpdateContClnt(ros::NodeHandle&); + ROSUnit_UpdateContClnt(ros::NodeHandle& nh, std::string Drone_Name); ~ROSUnit_UpdateContClnt(); }; diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index 2c0fc03..59d777c 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -7,7 +7,7 @@ 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); + m_server = nh_.advertiseService("set_height_offset", &ROSUnit_PoseProvider::srv_callback, this); rot_offset.setRPY(0.0, 0.0, M_PI/2.0); trans_offset.setZero(); diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index 0634995..1ccefb6 100755 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -2,30 +2,30 @@ namespace HEAR{ -ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler) : nh_(t_main_handler){ +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("update_controller/pid/x"); - m_client_pid_y = nh_.serviceClient("update_controller/pid/y"); - m_client_pid_z = nh_.serviceClient("update_controller/pid/z"); + 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("update_controller/pid/roll"); - m_client_pid_pitch = nh_.serviceClient("update_controller/pid/pitch"); - m_client_pid_yaw = nh_.serviceClient("update_controller/pid/yaw"); - m_client_pid_yaw_rate = nh_.serviceClient("update_controller/pid/yaw_rate"); + 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("update_controller/mrft/x"); - m_client_mrft_y = nh_.serviceClient("update_controller/mrft/y"); - m_client_mrft_z = nh_.serviceClient("update_controller/mrft/z"); + 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("update_controller/mrft/roll"); - m_client_mrft_pitch = nh_.serviceClient("update_controller/mrft/pitch"); - m_client_mrft_yaw = nh_.serviceClient("update_controller/mrft/yaw"); - m_client_mrft_yaw_rate = nh_.serviceClient("update_controller/mrft/yaw_rate"); -} + 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"); +} ROSUnit_UpdateContClnt::~ROSUnit_UpdateContClnt() { From 3e9aca003d381ac036fa85ca4b8174f854230e62 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Mon, 5 Dec 2022 12:05:00 +0400 Subject: [PATCH 15/36] Bounding Switch Service --- .../HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp | 74 +++++++++++++++++++ include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 4 + src/ROSUnit_UpdateContClnt.cpp | 53 ++++++++++++- 3 files changed, 129 insertions(+), 2 deletions(-) create mode 100644 include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp diff --git a/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp b/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp new file mode 100644 index 0000000..3960022 --- /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: + UpdateTrigger* _bounding_trig; + std::vector > _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 UpdateTrigger(); + } + ExternalTrigger* getBoundingTrig(){ return _bounding_trig;} + ExternalTrigger* registerSwitchTrig(bool inverted_logic = false){ + auto trig = new UpdateTrigger(); + _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((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); + } + + } + else{ + BoolMsg msg; + msg.data = true; + _bounding_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); + } + } + return true; +} + +} + +#endif diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp index bc1f014..c35d33c 100755 --- a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "HEAR_core/DataTypes.hpp" #include @@ -30,6 +31,9 @@ class ROSUnit_UpdateContClnt { 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); diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index 1ccefb6..1392add 100755 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -25,6 +25,11 @@ ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler, 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() { @@ -94,7 +99,7 @@ bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { return true; } else { - ROS_ERROR("Failed to call service /update_controller"); + ROS_ERROR("Failed to call service /update_controller/pid"); return false; } } @@ -158,7 +163,51 @@ bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID ID) { return true; } else { - ROS_ERROR("Failed to call service /update_controller"); + 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_o1; + 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; } } From 32c0d1a2c03d71e657a45893ede655ae96755834 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 13 Dec 2022 12:58:56 +0400 Subject: [PATCH 16/36] FloatArrSub, multiport, update bounding --- include/HEAR_ROS/ROSUnit_FloatArrSub.hpp | 31 ++++++ .../HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp | 104 ++++++++++++++++++ .../HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp | 26 +++++ include/HEAR_ROS/RosSystem.hpp | 14 ++- src/ROSUnit_UpdateBOUNDINGsrv.cpp | 22 ++++ 5 files changed, 195 insertions(+), 2 deletions(-) create mode 100644 include/HEAR_ROS/ROSUnit_FloatArrSub.hpp create mode 100644 include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp create mode 100644 include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp create mode 100644 src/ROSUnit_UpdateBOUNDINGsrv.cpp diff --git a/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp new file mode 100644 index 0000000..b605646 --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp @@ -0,0 +1,31 @@ +#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, 10, &ROSUnitFloatArrSub::callback, this); + _output_port = new OutputPort>(idx, 0); + + ((OutputPort>*)_output_port)->write({0,0,0,0,0,0}); + id_ = idx; + } + + TYPE getType(){ return TYPE::FloatVec;} + +}; + +} + +#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 100644 index 0000000..1b934d4 --- /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: + UpdateTrigger* _pid_trig; + UpdateTrigger* _mrft_trig; + UpdateTrigger* _bounding_trig; + std::vector > _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 UpdateTrigger(); + _mrft_trig = new UpdateTrigger(); + _bounding_trig = new UpdateTrigger(); + } + ExternalTrigger* getMRFTTrig(){ return _mrft_trig;} + ExternalTrigger* getPIDTrig(){ return _pid_trig;} + ExternalTrigger* getBoundingTrig(){ return _bounding_trig;} + ExternalTrigger* registerSwitchTrig(bool inverted_logic = false){ + auto trig = new UpdateTrigger(); + _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((UpdateMsg*)&msg); + _bounding_trig->UpdateCallback((UpdateMsg*)&msg); + msg.data = true; + _mrft_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); + } + + } + else if (data == 2){ + BoolMsg msg; + msg.data = false; + _mrft_trig->UpdateCallback((UpdateMsg*)&msg); + _pid_trig->UpdateCallback((UpdateMsg*)&msg); + msg.data = true; + _bounding_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); + } + } + else{ + BoolMsg msg; + msg.data = false; + _mrft_trig->UpdateCallback((UpdateMsg*)&msg); + _bounding_trig->UpdateCallback((UpdateMsg*)&msg); + msg.data = true; + _pid_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); + } + } + return true; +} + +} + +#endif diff --git a/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp new file mode 100644 index 0000000..b8ed493 --- /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; + UpdateTrigger* 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) {} + UpdateTrigger* 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 index 627d19a..5257676 100755 --- a/include/HEAR_ROS/RosSystem.hpp +++ b/include/HEAR_ROS/RosSystem.hpp @@ -7,12 +7,14 @@ #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_BoolSrv.hpp" #include "HEAR_ROS/ROSUnit_Sub.hpp" #include "HEAR_ROS/ROSUnit_PointSub.hpp" @@ -88,8 +90,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(pnh_, topic_name, pub_counter++); + break; default: - std::cout <<"invalid subscriber type" <registerServer(topic); break; } + case UPDATE_MSG_TYPE::BOUNDINGCTRL_UPDATE : { + auto srv = new ROSUnit_UpdateBOUNDINGsrv(pnh_); + trig = srv->registerServer(topic); + break; + } case UPDATE_MSG_TYPE::CONSTANT_UPDATE : { auto srv = new ROSUnit_FloatServer(pnh_); trig = srv->registerServer(topic); break; } default: - return NULL; + ROS_ERROR("RosSystem::createUpdateTrigger: Unknown update message type"); } this->addExternalTrigger(trig, topic); diff --git a/src/ROSUnit_UpdateBOUNDINGsrv.cpp b/src/ROSUnit_UpdateBOUNDINGsrv.cpp new file mode 100644 index 0000000..15d0b17 --- /dev/null +++ b/src/ROSUnit_UpdateBOUNDINGsrv.cpp @@ -0,0 +1,22 @@ +#include "HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp" + +namespace HEAR{ + +UpdateTrigger* ROSUnit_UpdateBOUNDINGsrv::registerServer(const std::string &service_topic){ + ext_trig = new UpdateTrigger; + 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((UpdateMsg*)&msg); + return true; +} + +} \ No newline at end of file From 46ff0c49268379bc7bfa9933a787418ea6c7acfb Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 13 Dec 2022 18:40:51 +0400 Subject: [PATCH 17/36] Add initial commands 1000 --- include/HEAR_ROS/ROSUnit_FloatArrSub.hpp | 6 +++++- include/HEAR_ROS/RosSystem.hpp | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp index b605646..133af0d 100644 --- a/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp @@ -17,8 +17,12 @@ class ROSUnitFloatArrSub : public ROSUnit_Sub{ ROSUnitFloatArrSub(ros::NodeHandle& nh, const std::string& topic_name, int idx){ sub_ = nh.subscribe(topic_name, 10, &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({0,0,0,0,0,0}); + ((OutputPort>*)_output_port)->write(_commands); id_ = idx; } diff --git a/include/HEAR_ROS/RosSystem.hpp b/include/HEAR_ROS/RosSystem.hpp index 5257676..4764de6 100755 --- a/include/HEAR_ROS/RosSystem.hpp +++ b/include/HEAR_ROS/RosSystem.hpp @@ -91,7 +91,7 @@ ROSUnit_Sub* RosSystem::createSub(TYPE d_type, std::string topic_name){ sub = new ROSUnitFloatSub(nh_, topic_name, sub_counter++); break; case TYPE::FloatVec : - sub = new ROSUnitFloatArrSub(pnh_, topic_name, pub_counter++); + sub = new ROSUnitFloatArrSub(nh_, topic_name, sub_counter++); break; default: std::cout <<"RosSystem: invalid subscriber type" < Date: Fri, 16 Dec 2022 09:02:42 +0400 Subject: [PATCH 18/36] param.h_o1 to param.h_o2 --- src/ROSUnit_UpdateContClnt.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index 1392add..ae31715 100755 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -174,7 +174,7 @@ bool ROSUnit_UpdateContClnt::process(UpdateMsg* t_msg, BLOCK_ID 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_o1; + srv.request.controller_parameters.bounding_h_o2 = _update_msg->param.h_o2; bool success = false; switch((int)_update_msg->param.id) From 8e97b965b54367324e1fb7d60eda9eb99191d689 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 21 Mar 2023 11:48:16 +0400 Subject: [PATCH 19/36] Modified Ros Package for (V3.0) --- hear_ros.cmake | 0 include/HEAR_ROS/ROSUnit_BoolSrv.hpp | 4 +- .../HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp | 126 ++++++------ include/HEAR_ROS/ROSUnit_FloatArrSub.hpp | 0 include/HEAR_ROS/ROSUnit_FloatSrv.hpp | 4 +- include/HEAR_ROS/ROSUnit_IntClnt.hpp | 22 +++ include/HEAR_ROS/ROSUnit_IntSrv.hpp | 25 +++ .../HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp | 186 +++++++++--------- include/HEAR_ROS/ROSUnit_ResetSrv.hpp | 8 +- .../HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp | 4 +- .../ROSUnit_UpdateBoundingContClnt.hpp | 26 +++ include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp | 4 +- .../HEAR_ROS/ROSUnit_UpdateMRFTContClnt.hpp | 27 +++ include/HEAR_ROS/ROSUnit_UpdateMRFTsrv.hpp | 4 +- .../HEAR_ROS/ROSUnit_UpdatePIDContClnt.hpp | 27 +++ .../HEAR_ROS/ROSUnit_UpdateTrajectoryClnt.hpp | 26 +++ .../HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp | 26 +++ include/HEAR_ROS/RosSystem.hpp | 44 +++-- include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp | 140 ++++++------- src/ROSUnit_BoolSrv.cpp | 6 +- src/ROSUnit_FloatSrv.cpp | 8 +- src/ROSUnit_IntClnt.cpp | 15 ++ src/ROSUnit_IntSrv.cpp | 21 ++ src/ROSUnit_ResetSrv.cpp | 11 +- src/ROSUnit_UpdateBOUNDINGsrv.cpp | 6 +- src/ROSUnit_UpdateBoundingContClnt.cpp | 37 ++++ src/ROSUnit_UpdateContSrv.cpp | 6 +- src/ROSUnit_UpdateMRFTContClnt.cpp | 38 ++++ src/ROSUnit_UpdateMRFTsrv.cpp | 6 +- src/ROSUnit_UpdatePIDContClnt.cpp | 41 ++++ src/ROSUnit_UpdateTrajectoryClnt.cpp | 40 ++++ src/ROSUnit_UpdateTrajectorySrv.cpp | 25 +++ 32 files changed, 686 insertions(+), 277 deletions(-) mode change 100644 => 100755 hear_ros.cmake mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_FloatArrSub.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_FloatSrv.hpp create mode 100755 include/HEAR_ROS/ROSUnit_IntClnt.hpp create mode 100755 include/HEAR_ROS/ROSUnit_IntSrv.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp mode change 100644 => 100755 include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp create mode 100755 include/HEAR_ROS/ROSUnit_UpdateBoundingContClnt.hpp create mode 100755 include/HEAR_ROS/ROSUnit_UpdateMRFTContClnt.hpp create mode 100755 include/HEAR_ROS/ROSUnit_UpdatePIDContClnt.hpp create mode 100755 include/HEAR_ROS/ROSUnit_UpdateTrajectoryClnt.hpp create mode 100755 include/HEAR_ROS/ROSUnit_UpdateTrajectorySrv.hpp mode change 100644 => 100755 src/ROSUnit_FloatSrv.cpp create mode 100755 src/ROSUnit_IntClnt.cpp create mode 100755 src/ROSUnit_IntSrv.cpp mode change 100644 => 100755 src/ROSUnit_UpdateBOUNDINGsrv.cpp create mode 100755 src/ROSUnit_UpdateBoundingContClnt.cpp create mode 100755 src/ROSUnit_UpdateMRFTContClnt.cpp create mode 100755 src/ROSUnit_UpdatePIDContClnt.cpp create mode 100755 src/ROSUnit_UpdateTrajectoryClnt.cpp create mode 100755 src/ROSUnit_UpdateTrajectorySrv.cpp diff --git a/hear_ros.cmake b/hear_ros.cmake old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_BoolSrv.hpp b/include/HEAR_ROS/ROSUnit_BoolSrv.hpp index 62229ff..54bbbf5 100755 --- 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 old mode 100644 new mode 100755 index 3960022..d05041f --- a/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp +++ b/include/HEAR_ROS/ROSUnit_BoundingSwitchSrv.hpp @@ -1,74 +1,74 @@ -#ifndef ROSUNIT_BOUNDINGSWITCHSRV_HPP -#define ROSUNIT_BOUNDINGSWITCHSRV_HPP +// #ifndef ROSUNIT_BOUNDINGSWITCHSRV_HPP +// #define ROSUNIT_BOUNDINGSWITCHSRV_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_BoundingSwitchSrv { -private: - UpdateTrigger* _bounding_trig; - std::vector > _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 UpdateTrigger(); - } - ExternalTrigger* getBoundingTrig(){ return _bounding_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_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((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_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((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); - } - } - return true; -} +// } +// 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 +// #endif diff --git a/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp old mode 100644 new mode 100755 diff --git a/include/HEAR_ROS/ROSUnit_FloatSrv.hpp b/include/HEAR_ROS/ROSUnit_FloatSrv.hpp old mode 100644 new mode 100755 index b367820..252ca29 --- a/include/HEAR_ROS/ROSUnit_FloatSrv.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatSrv.hpp @@ -12,11 +12,11 @@ class ROSUnit_FloatServer { private: ros::NodeHandle nh_; ros::ServiceServer m_server; - UpdateTrigger* ext_trig; + ExternalTrigger* ext_trig; bool srv_callback(hear_msgs::set_float::Request&, hear_msgs::set_float::Response&); public: ROSUnit_FloatServer(ros::NodeHandle&); - UpdateTrigger* registerServer(const std::string&); + ExternalTrigger* registerServer(const std::string&); }; 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 old mode 100644 new mode 100755 index 1b934d4..a51f4dc --- a/include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp +++ b/include/HEAR_ROS/ROSUnit_MultiportSwitchSrv.hpp @@ -1,104 +1,104 @@ -#ifndef ROSUNIT_MULTIPORTSWITCHSRV_HPP -#define ROSUNIT_MULTIPORTSWITCHSRV_HPP +// #ifndef ROSUNIT_MULTIPORTSWITCHSRV_HPP +// #define ROSUNIT_MULTIPORTSWITCHSRV_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_MultiportSwitchSrv { -private: - UpdateTrigger* _pid_trig; - UpdateTrigger* _mrft_trig; - UpdateTrigger* _bounding_trig; - std::vector > _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 UpdateTrigger(); - _mrft_trig = new UpdateTrigger(); - _bounding_trig = new UpdateTrigger(); - } - ExternalTrigger* getMRFTTrig(){ return _mrft_trig;} - ExternalTrigger* getPIDTrig(){ return _pid_trig;} - ExternalTrigger* getBoundingTrig(){ return _bounding_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_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((UpdateMsg*)&msg); - _bounding_trig->UpdateCallback((UpdateMsg*)&msg); - msg.data = true; - _mrft_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); - } +// 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((UpdateMsg*)&msg); - _pid_trig->UpdateCallback((UpdateMsg*)&msg); - msg.data = true; - _bounding_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); - } - } - else{ - BoolMsg msg; - msg.data = false; - _mrft_trig->UpdateCallback((UpdateMsg*)&msg); - _bounding_trig->UpdateCallback((UpdateMsg*)&msg); - msg.data = true; - _pid_trig->UpdateCallback((UpdateMsg*)&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((UpdateMsg*)&sw_msg); - } - } - return true; -} +// } +// 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 +// #endif diff --git a/include/HEAR_ROS/ROSUnit_ResetSrv.hpp b/include/HEAR_ROS/ROSUnit_ResetSrv.hpp index ffc2710..f262778 100755 --- 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_UpdateBOUNDINGsrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp old mode 100644 new mode 100755 index b8ed493..5e00d6f --- a/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateBOUNDINGsrv.hpp @@ -13,11 +13,11 @@ class ROSUnit_UpdateBOUNDINGsrv { private: ros::NodeHandle nh_; ros::ServiceServer m_server; - UpdateTrigger* ext_trig; + 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) {} - UpdateTrigger* registerServer(const std::string&); + ExternalTrigger* registerServer(const std::string&); }; 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_UpdateContSrv.hpp b/include/HEAR_ROS/ROSUnit_UpdateContSrv.hpp index 27a0bc1..37353a9 100755 --- 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 index 39aeaba..85a40be 100755 --- 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 index 4764de6..b35a83f 100755 --- a/include/HEAR_ROS/RosSystem.hpp +++ b/include/HEAR_ROS/RosSystem.hpp @@ -15,7 +15,9 @@ #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" @@ -37,10 +39,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_; @@ -153,43 +155,53 @@ void RosSystem::createPub(std::string topic_name, OutputPort* 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 UPDATE_MSG_TYPE::BOUNDINGCTRL_UPDATE : { + case MSG_TYPE::BOUNDINGCTRL_UPDATE : { auto srv = new ROSUnit_UpdateBOUNDINGsrv(pnh_); trig = srv->registerServer(topic); break; } - case UPDATE_MSG_TYPE::CONSTANT_UPDATE : { + 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; @@ -202,9 +214,9 @@ ExternalTrigger* RosSystem::createUpdateTrigger(UPDATE_MSG_TYPE type, std::strin 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 index d1dcc14..806f168 100755 --- a/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp +++ b/include/HEAR_ROS/RosUnit_MRFTSwitchSrv.hpp @@ -1,81 +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); - } - } - return true; -} +// } +// 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 +// #endif diff --git a/src/ROSUnit_BoolSrv.cpp b/src/ROSUnit_BoolSrv.cpp index 75afe95..54cd78b 100755 --- 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_FloatSrv.cpp b/src/ROSUnit_FloatSrv.cpp old mode 100644 new mode 100755 index ee5002b..80e0555 --- a/src/ROSUnit_FloatSrv.cpp +++ b/src/ROSUnit_FloatSrv.cpp @@ -3,18 +3,18 @@ namespace HEAR{ ROSUnit_FloatServer::ROSUnit_FloatServer(ros::NodeHandle &nh) : nh_(nh){ - ext_trig = new UpdateTrigger; + ext_trig = new ExternalTrigger; } -UpdateTrigger* ROSUnit_FloatServer::registerServer(const std::string &service_topic){ +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){ - Variable_UpdateMsg msg; + FloatMsg msg; msg._value = req.data; - ext_trig->UpdateCallback((UpdateMsg*)&msg); + ext_trig->UpdateCallback((BaseMsg*)&msg); return true; } 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_ResetSrv.cpp b/src/ROSUnit_ResetSrv.cpp index 9b43b00..f6a883a 100755 --- 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_UpdateBOUNDINGsrv.cpp b/src/ROSUnit_UpdateBOUNDINGsrv.cpp old mode 100644 new mode 100755 index 15d0b17..a10a641 --- a/src/ROSUnit_UpdateBOUNDINGsrv.cpp +++ b/src/ROSUnit_UpdateBOUNDINGsrv.cpp @@ -2,8 +2,8 @@ namespace HEAR{ -UpdateTrigger* ROSUnit_UpdateBOUNDINGsrv::registerServer(const std::string &service_topic){ - ext_trig = new UpdateTrigger; +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; } @@ -15,7 +15,7 @@ bool ROSUnit_UpdateBOUNDINGsrv::srv_callback(hear_msgs::Update_Controller_Boundi 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((UpdateMsg*)&msg); + ext_trig->UpdateCallback((BaseMsg*)&msg); return true; } 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_UpdateContSrv.cpp b/src/ROSUnit_UpdateContSrv.cpp index ca34698..d4d5e1e 100755 --- 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 index e55761d..df3a9fc 100755 --- 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..373c257 --- /dev/null +++ b/src/ROSUnit_UpdateTrajectoryClnt.cpp @@ -0,0 +1,40 @@ +#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.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.NumSamples = _update_msg->param.NumSamples; + 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..2471214 --- /dev/null +++ b/src/ROSUnit_UpdateTrajectorySrv.cpp @@ -0,0 +1,25 @@ +#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._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.NumSamples = req.trajectory_parameters.NumSamples; + msg.param.ClearQ = req.trajectory_parameters.ClearQ; + + ext_trig->UpdateCallback((BaseMsg*)&msg); + return true; +} + +} \ No newline at end of file From a87a719519bd8735f34a28ab378d829a91e553f8 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 21 Mar 2023 12:14:10 +0400 Subject: [PATCH 20/36] Comment ROSUnit_UpdateContClnt.hpp --- include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp | 76 ++-- src/ROSUnit_UpdateContClnt.cpp | 408 ++++++++++---------- 2 files changed, 242 insertions(+), 242 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp index c35d33c..803ffc3 100755 --- a/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_UpdateContClnt.hpp @@ -1,46 +1,46 @@ -#ifndef ROSUNIT_UPDATECONTCLNT_HPP -#define ROSUNIT_UPDATECONTCLNT_HPP +// #ifndef ROSUNIT_UPDATECONTCLNT_HPP +// #define ROSUNIT_UPDATECONTCLNT_HPP -#include -#include -#include -#include -#include -#include -#include "HEAR_core/DataTypes.hpp" -#include +// #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(); +// 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 +// #endif \ No newline at end of file diff --git a/src/ROSUnit_UpdateContClnt.cpp b/src/ROSUnit_UpdateContClnt.cpp index ae31715..ee26b54 100755 --- a/src/ROSUnit_UpdateContClnt.cpp +++ b/src/ROSUnit_UpdateContClnt.cpp @@ -1,220 +1,220 @@ -#include "HEAR_ROS/ROSUnit_UpdateContClnt.hpp" +// #include "HEAR_ROS/ROSUnit_UpdateContClnt.hpp" -namespace HEAR{ +// namespace HEAR{ -ROSUnit_UpdateContClnt::ROSUnit_UpdateContClnt(ros::NodeHandle& t_main_handler, std::string Drone_Name) : nh_(t_main_handler){ +// 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 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"); +// // 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 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"); +// // 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"); -} +// // 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() { +// 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; +// 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)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" <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; +// } +// 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; +// } +// } +// 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" <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; - } -} +// } +// 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; +// } +// } +// else{ +// ROS_ERROR("Failed to update controller"); +// return false; +// } +// } -} +// } From 0b60f92b153f3e8d501391efbadb042339f328ae Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Sun, 30 Apr 2023 11:24:25 +0400 Subject: [PATCH 21/36] Add samplingType --- src/ROSUnit_UpdateTrajectoryClnt.cpp | 1 + src/ROSUnit_UpdateTrajectorySrv.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/ROSUnit_UpdateTrajectoryClnt.cpp b/src/ROSUnit_UpdateTrajectoryClnt.cpp index 373c257..b2598c8 100755 --- a/src/ROSUnit_UpdateTrajectoryClnt.cpp +++ b/src/ROSUnit_UpdateTrajectoryClnt.cpp @@ -17,6 +17,7 @@ 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; diff --git a/src/ROSUnit_UpdateTrajectorySrv.cpp b/src/ROSUnit_UpdateTrajectorySrv.cpp index 2471214..9b27805 100755 --- a/src/ROSUnit_UpdateTrajectorySrv.cpp +++ b/src/ROSUnit_UpdateTrajectorySrv.cpp @@ -11,6 +11,7 @@ ExternalTrigger* ROSUnit_UpdateTrajectorySrv::registerServer(const std: 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; From e8b12e7a1fa793c1694ffa974ab0a2c650f5e3db Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Sun, 30 Apr 2023 17:04:50 +0400 Subject: [PATCH 22/36] Add Float3 Client --- include/HEAR_ROS/ROSUnit_Float3Clnt.hpp | 21 +++++++++++++++++++++ src/ROSUnit_Float3Clnt.cpp | 17 +++++++++++++++++ 2 files changed, 38 insertions(+) create mode 100644 include/HEAR_ROS/ROSUnit_Float3Clnt.hpp create mode 100644 src/ROSUnit_Float3Clnt.cpp 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/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 From 5777304412a0c1bfb3bc0dc9097e01070d0919a9 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Sun, 30 Apr 2023 17:17:53 +0400 Subject: [PATCH 23/36] Add Float3 Service --- include/HEAR_ROS/ROSUnit_Float3Srv.hpp | 25 +++++++++++++++++++++++++ include/HEAR_ROS/RosSystem.hpp | 6 ++++++ src/ROSUnit_Float3Srv.cpp | 23 +++++++++++++++++++++++ 3 files changed, 54 insertions(+) create mode 100644 include/HEAR_ROS/ROSUnit_Float3Srv.hpp create mode 100644 src/ROSUnit_Float3Srv.cpp 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/RosSystem.hpp b/include/HEAR_ROS/RosSystem.hpp index b35a83f..c0629b8 100755 --- a/include/HEAR_ROS/RosSystem.hpp +++ b/include/HEAR_ROS/RosSystem.hpp @@ -23,6 +23,7 @@ #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{ @@ -206,6 +207,11 @@ ExternalTrigger* RosSystem::createUpdateTrigger(MSG_TYPE type, std::str trig = srv->registerServer(topic); break; } + case MSG_TYPE::FLOAT3_MSG : { + auto srv = new ROSUnit_Float3Server(pnh_); + trig = srv->registerServer(topic); + break; + } default: ROS_ERROR("RosSystem::createUpdateTrigger: Unknown update message type"); } 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 From 1748b4fcdf4368dfbf291d8f0f200dceaed90f1e Mon Sep 17 00:00:00 2001 From: mchehadeh Date: Wed, 17 May 2023 17:00:51 +0400 Subject: [PATCH 24/36] buffer set to 1 --- include/HEAR_ROS/ROSUnit_FloatArrPub.hpp | 2 +- include/HEAR_ROS/ROSUnit_FloatArrSub.hpp | 2 +- include/HEAR_ROS/ROSUnit_FloatPub.hpp | 2 +- include/HEAR_ROS/ROSUnit_FloatSub.hpp | 2 +- include/HEAR_ROS/ROSUnit_PointPub.hpp | 2 +- include/HEAR_ROS/ROSUnit_PointSub.hpp | 2 +- include/HEAR_ROS/ROSUnit_QuatPub.hpp | 2 +- include/HEAR_ROS/ROSUnit_QuatSub.hpp | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp b/include/HEAR_ROS/ROSUnit_FloatArrPub.hpp index a008e65..7a01566 100755 --- 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 index 133af0d..034a426 100755 --- a/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp +++ b/include/HEAR_ROS/ROSUnit_FloatArrSub.hpp @@ -15,7 +15,7 @@ class ROSUnitFloatArrSub : public ROSUnit_Sub{ } public: ROSUnitFloatArrSub(ros::NodeHandle& nh, const std::string& topic_name, int idx){ - sub_ = nh.subscribe(topic_name, 10, &ROSUnitFloatArrSub::callback, this); + 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++){ diff --git a/include/HEAR_ROS/ROSUnit_FloatPub.hpp b/include/HEAR_ROS/ROSUnit_FloatPub.hpp index 9cc1b92..cba7ac3 100755 --- 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_FloatSub.hpp b/include/HEAR_ROS/ROSUnit_FloatSub.hpp index 8dd0e34..7f0b642 100755 --- 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_PointPub.hpp b/include/HEAR_ROS/ROSUnit_PointPub.hpp index 1509220..9f9c0d0 100755 --- 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 index 6142074..eda858c 100755 --- 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_QuatPub.hpp b/include/HEAR_ROS/ROSUnit_QuatPub.hpp index 43c2453..e414d73 100755 --- 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 index ba5445a..eed5ee0 100755 --- 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; } From 8d74fb810d91bf1e247af8fed98cc55b84f10d3a Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 31 May 2023 16:21:42 +0400 Subject: [PATCH 25/36] Add TotalExecutionTime instead of NumSamples --- src/ROSUnit_UpdateTrajectoryClnt.cpp | 3 ++- src/ROSUnit_UpdateTrajectorySrv.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/ROSUnit_UpdateTrajectoryClnt.cpp b/src/ROSUnit_UpdateTrajectoryClnt.cpp index b2598c8..8df82dd 100755 --- a/src/ROSUnit_UpdateTrajectoryClnt.cpp +++ b/src/ROSUnit_UpdateTrajectoryClnt.cpp @@ -22,7 +22,8 @@ bool ROSUnit_UpdateTrajectoryClnt::process(BaseMsg* t_msg) { 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.NumSamples = _update_msg->param.NumSamples; + // srv.request.trajectory_parameters.NumSamples = _update_msg->param.NumSamples; + srv.request.trajectory_parameters.TotalExecutionTime = _update_msg->param.TotalExecutionTime; srv.request.trajectory_parameters.ClearQ = _update_msg->param.ClearQ; bool success = false; success = m_client.call(srv); diff --git a/src/ROSUnit_UpdateTrajectorySrv.cpp b/src/ROSUnit_UpdateTrajectorySrv.cpp index 9b27805..0ec730f 100755 --- a/src/ROSUnit_UpdateTrajectorySrv.cpp +++ b/src/ROSUnit_UpdateTrajectorySrv.cpp @@ -16,7 +16,8 @@ bool ROSUnit_UpdateTrajectorySrv::srv_callback(hear_msgs::Update_Trajectory::Req msg.param.rot = req.trajectory_parameters.rot; msg.param.trans = req.trajectory_parameters.trans; msg.param.scale = req.trajectory_parameters.scale; - msg.param.NumSamples = req.trajectory_parameters.NumSamples; + // msg.param.NumSamples = req.trajectory_parameters.NumSamples; + msg.param.TotalExecutionTime = req.trajectory_parameters.TotalExecutionTime; msg.param.ClearQ = req.trajectory_parameters.ClearQ; ext_trig->UpdateCallback((BaseMsg*)&msg); From cbb52a7520778f12f214a932c81d64acf23b01cb Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Thu, 1 Jun 2023 12:17:42 +0400 Subject: [PATCH 26/36] Set Const Velocity from MC --- src/ROSUnit_UpdateTrajectoryClnt.cpp | 2 +- src/ROSUnit_UpdateTrajectorySrv.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ROSUnit_UpdateTrajectoryClnt.cpp b/src/ROSUnit_UpdateTrajectoryClnt.cpp index 8df82dd..df93fff 100755 --- a/src/ROSUnit_UpdateTrajectoryClnt.cpp +++ b/src/ROSUnit_UpdateTrajectoryClnt.cpp @@ -22,8 +22,8 @@ bool ROSUnit_UpdateTrajectoryClnt::process(BaseMsg* t_msg) { 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.NumSamples = _update_msg->param.NumSamples; 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); diff --git a/src/ROSUnit_UpdateTrajectorySrv.cpp b/src/ROSUnit_UpdateTrajectorySrv.cpp index 0ec730f..74ab9c2 100755 --- a/src/ROSUnit_UpdateTrajectorySrv.cpp +++ b/src/ROSUnit_UpdateTrajectorySrv.cpp @@ -16,8 +16,8 @@ bool ROSUnit_UpdateTrajectorySrv::srv_callback(hear_msgs::Update_Trajectory::Req msg.param.rot = req.trajectory_parameters.rot; msg.param.trans = req.trajectory_parameters.trans; msg.param.scale = req.trajectory_parameters.scale; - // msg.param.NumSamples = req.trajectory_parameters.NumSamples; 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); From 23c7196e43833c42b2e507218ffe6761b6fd556e Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Fri, 16 Jun 2023 11:56:57 +0400 Subject: [PATCH 27/36] comment ROSUnit_SLAM for tf2 error --- include/HEAR_ROS/ROSUnit_SLAM.hpp | 102 ++++++++--------- src/ROSUnit_SLAM.cpp | 182 +++++++++++++++--------------- 2 files changed, 142 insertions(+), 142 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_SLAM.hpp b/include/HEAR_ROS/ROSUnit_SLAM.hpp index 0fa7298..acdc0df 100755 --- 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/src/ROSUnit_SLAM.cpp b/src/ROSUnit_SLAM.cpp index fb5fdbb..62d6547 100755 --- 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 From a23fd0915238be8eb5d29dc1cf89241ac0e37f64 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 21 Jun 2023 08:43:43 +0100 Subject: [PATCH 28/36] Comment for Pi4 --- include/HEAR_ROS/ROSUnit_SLAM.hpp | 102 ++++++++--------- src/ROSUnit_SLAM.cpp | 182 +++++++++++++++--------------- 2 files changed, 142 insertions(+), 142 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_SLAM.hpp b/include/HEAR_ROS/ROSUnit_SLAM.hpp index 0fa7298..acdc0df 100755 --- 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/src/ROSUnit_SLAM.cpp b/src/ROSUnit_SLAM.cpp index fb5fdbb..62d6547 100755 --- 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 From 41f6446fb28ecc12e9d967b1eafcc3ae8048b302 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 19 Jul 2023 18:49:38 +0400 Subject: [PATCH 29/36] Add Vision pose --- include/HEAR_ROS/ROSUnit_PoseProvider.hpp | 16 ++++-- src/ROSUnit_PoseProvider.cpp | 64 +++++++++++++++++++++++ 2 files changed, 76 insertions(+), 4 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp index 49a6435..8423cc2 100755 --- a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include #include @@ -22,26 +23,32 @@ 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, 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>* vision_pos_port; + ExternalOutputPort>* vision_vel_port; + ExternalOutputPort>* vision_ori_port; ExternalOutputPort>* imu_ori_port; ExternalOutputPort>* imu_acc_port; ExternalOutputPort>* 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&); 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; + 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 +56,7 @@ 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>* registerImuOri(std::string t_name); ExternalOutputPort>* registerImuAngularRate(std::string t_name); ExternalOutputPort>* registerImuAcceleration(std::string t_name); diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index 59d777c..c8b7add 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -21,6 +21,20 @@ std::vector>*> ROSUnit_PoseProvider::register 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, M_PI/2.0); + trans_offset_vision.setZero(); + + vision_pos_port = new ExternalOutputPort>(0); + vision_pos_port->write(Vector3D(0,0,0)); + vision_vel_port = new ExternalOutputPort>(0); + vision_vel_port->write(Vector3D(0,0,0)); + vision_ori_port = new ExternalOutputPort>(0); + 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}; +} + ExternalOutputPort>* ROSUnit_PoseProvider::registerImuOri(std::string t_name){ imu_ori_port = new ExternalOutputPort>(0); imu_ori_port->write(Vector3D(0,0,0)); @@ -97,6 +111,56 @@ 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)); From 50473501b26759c77a793a4a806c8f162bb5b2f5 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 9 Aug 2023 14:42:25 +0400 Subject: [PATCH 30/36] Optitrack Convention --- src/ROSUnit_PoseProvider.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index c8b7add..f095245 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -8,7 +8,7 @@ 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); + rot_offset.setRPY(M_PI/2.0, 0.0, M_PI/2.0); trans_offset.setZero(); opti_pos_port = new ExternalOutputPort>(0); @@ -22,7 +22,7 @@ std::vector>*> ROSUnit_PoseProvider::register } std::vector>*> ROSUnit_PoseProvider::registerVisionPose(std::string t_name){ - rot_offset_vision.setRPY(0.0, 0.0, M_PI/2.0); + rot_offset_vision.setRPY(M_PI/2.0, 0.0, M_PI/2.0); trans_offset_vision.setZero(); vision_pos_port = new ExternalOutputPort>(0); From a84d655b358c534a7198f43379dedccfb438aa75 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Tue, 15 Aug 2023 10:57:11 +0400 Subject: [PATCH 31/36] fix vision orientation --- src/ROSUnit_PoseProvider.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index f095245..7181a1a 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -22,7 +22,7 @@ std::vector>*> ROSUnit_PoseProvider::register } std::vector>*> ROSUnit_PoseProvider::registerVisionPose(std::string t_name){ - rot_offset_vision.setRPY(M_PI/2.0, 0.0, M_PI/2.0); + rot_offset_vision.setRPY(0.0, 0.0, 0.0); trans_offset_vision.setZero(); vision_pos_port = new ExternalOutputPort>(0); From bb0e368228957e0e3209d753858440bd1d60fb54 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Mon, 21 Aug 2023 20:40:50 +0400 Subject: [PATCH 32/36] remove b_uid --- src/ROSUnit_PoseProvider.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index 7181a1a..58ba36f 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -11,11 +11,11 @@ std::vector>*> ROSUnit_PoseProvider::register rot_offset.setRPY(M_PI/2.0, 0.0, M_PI/2.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}; @@ -25,32 +25,32 @@ std::vector>*> ROSUnit_PoseProvider::register rot_offset_vision.setRPY(0.0, 0.0, 0.0); trans_offset_vision.setZero(); - vision_pos_port = new ExternalOutputPort>(0); + vision_pos_port = new ExternalOutputPort>(); vision_pos_port->write(Vector3D(0,0,0)); - vision_vel_port = new ExternalOutputPort>(0); + vision_vel_port = new ExternalOutputPort>(); vision_vel_port->write(Vector3D(0,0,0)); - vision_ori_port = new ExternalOutputPort>(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}; } 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; From 3fefd531f0afcc7db8d87a31e096be19c37f35de Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 30 Aug 2023 08:07:23 +0100 Subject: [PATCH 33/36] Successful flight test PX4 --- hear_ros.cmake | 2 + include/HEAR_ROS/ROSUnit_BoolClnt.hpp | 1 - include/HEAR_ROS/ROSUnit_EmptyClnt.hpp | 28 +++---- .../ROSUnit_PX4AngularVelocittyProvider.hpp | 36 +++++++++ include/HEAR_ROS/ROSUnit_PoseProvider.hpp | 8 +- src/ROSUnit_EmptyClnt.cpp | 20 ++--- src/ROSUnit_PoseProvider.cpp | 80 ++++++++++++++++++- 7 files changed, 148 insertions(+), 27 deletions(-) create mode 100755 include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp diff --git a/hear_ros.cmake b/hear_ros.cmake index c103006..49ca34b 100755 --- a/hear_ros.cmake +++ b/hear_ros.cmake @@ -6,6 +6,8 @@ file(GLOB HEAR_ROS_SRCs ${HEAR_ROS_SOURCE_DIR}/*.cpp) find_package(catkin REQUIRED COMPONENTS roscpp hear_msgs + mavros_msgs + std_srvs geometry_msgs tf2_geometry_msgs tf2 diff --git a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp index d2d3597..20f5b69 100755 --- a/include/HEAR_ROS/ROSUnit_BoolClnt.hpp +++ b/include/HEAR_ROS/ROSUnit_BoolClnt.hpp @@ -3,7 +3,6 @@ #include #include -#include #include "HEAR_core/DataTypes.hpp" #include "HEAR_ROS/ROSUnit_Client.hpp" #include diff --git a/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp b/include/HEAR_ROS/ROSUnit_EmptyClnt.hpp index f8eed03..b90c545 100755 --- 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 "HEAR_ROS/ROSUnit_Client.hpp" +// #include +// #include +// #include "HEAR_ROS/ROSUnit_Client.hpp" -namespace HEAR{ +// namespace HEAR{ -class ROSUnitEmptyClient: public ROSUnit_Client{ -public: - ros::NodeHandle nh_; - 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_PX4AngularVelocittyProvider.hpp b/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp new file mode 100755 index 0000000..0e57bfd --- /dev/null +++ b/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp @@ -0,0 +1,36 @@ +#ifndef ROSUNIT_PX4ANGULARVELOCITYPROVIDER_HPP +#define ROSUNIT_PX4ANGULARVELOCITYPROVIDER_HPP + +#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; + void callback_angular_vel(const mavros_msgs::VehicleAngularVelocity::ConstPtr&); + +public: + void process(){} + ROSUnit_PX4AngularVelocittyProvider(ros::NodeHandle& nh); + ~ROSUnit_PX4AngularVelocittyProvider(){} + ExternalOutputPort>* registerImuAngularRate(std::string t_name); +}; + +} + +#endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp index 8423cc2..2feed88 100755 --- a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp @@ -4,6 +4,7 @@ #include #include "ros/ros.h" +#include #include #include #include @@ -23,7 +24,7 @@ namespace HEAR{ class ROSUnit_PoseProvider{ private: ros::NodeHandle nh_; - ros::Subscriber opti_sub, vision_sub, xsens_ori_sub, xsens_ang_vel_sub, xsens_free_acc_sub; + ros::Subscriber opti_sub, vision_sub, px4_ang_vel_sub, xsens_ori_sub, xsens_ang_vel_sub, xsens_free_acc_sub; ros::ServiceServer m_server; ExternalOutputPort>* opti_pos_port; @@ -35,11 +36,14 @@ class ROSUnit_PoseProvider{ 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_px4_pose(const geometry_msgs::PoseStamped::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&); + void callback_px4_angular_vel(const mavros_msgs::VehicleAngularVelocity::ConstPtr&); bool srv_callback(hear_msgs::set_float::Request&, hear_msgs::set_float::Response&); tf2::Matrix3x3 rot_offset, rot_offset_vision; tf2::Vector3 trans_offset, trans_offset_vision; @@ -57,8 +61,10 @@ class ROSUnit_PoseProvider{ ~ROSUnit_PoseProvider(){} std::vector>*> registerOptiPose(std::string t_name); std::vector>*> registerVisionPose(std::string t_name); + std::vector>*> registerPX4Pose(std::string t_name); ExternalOutputPort>* registerImuOri(std::string t_name); ExternalOutputPort>* registerImuAngularRate(std::string t_name); + ExternalOutputPort>* registerPX4ImuAngularRate(std::string t_name); ExternalOutputPort>* registerImuAcceleration(std::string t_name); }; diff --git a/src/ROSUnit_EmptyClnt.cpp b/src/ROSUnit_EmptyClnt.cpp index e4a57f7..3dcf600 100755 --- 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) : nh_(nh){ - 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_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index 58ba36f..8f53332 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -8,7 +8,7 @@ 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(M_PI/2.0, 0.0, M_PI/2.0); + rot_offset.setRPY(0.0, 0.0, 0.0); trans_offset.setZero(); opti_pos_port = new ExternalOutputPort>(); @@ -35,6 +35,21 @@ std::vector>*> ROSUnit_PoseProvider::register return std::vector>*>{vision_pos_port, vision_vel_port, vision_ori_port}; } +std::vector>*> 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(); + + opti_pos_port = new ExternalOutputPort>(); + opti_pos_port->write(Vector3D(0,0,0)); + opti_vel_port = new ExternalOutputPort>(); + opti_vel_port->write(Vector3D(0,0,0)); + opti_ori_port = new ExternalOutputPort>(); + opti_ori_port->write(Vector3D(0,0,0)); + opti_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_px4_pose, this, ros::TransportHints().tcpNoDelay()); + return std::vector>*>{opti_pos_port, opti_vel_port, opti_ori_port}; +} + ExternalOutputPort>* ROSUnit_PoseProvider::registerImuOri(std::string t_name){ imu_ori_port = new ExternalOutputPort>(); imu_ori_port->write(Vector3D(0,0,0)); @@ -49,6 +64,13 @@ ExternalOutputPort>* ROSUnit_PoseProvider::registerImuAngularRat return imu_angular_rt_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; +} + ExternalOutputPort>* ROSUnit_PoseProvider::registerImuAcceleration(std::string t_name){ imu_acc_port = new ExternalOutputPort>(); imu_acc_port->write(Vector3D(0,0,0)); @@ -161,6 +183,56 @@ void ROSUnit_PoseProvider::callback_vision_pose(const geometry_msgs::PoseWithCov vision_ori_port->write(vec_ori); } +void ROSUnit_PoseProvider::callback_px4_pose(const geometry_msgs::PoseStamped::ConstPtr& msg){ + + tf2::Vector3 vel; + auto pos = tf2::Vector3({msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}); + auto calib_pos = 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}; + + // velocity calculation + if(first_read == 0){ + first_read = 1; + prevT = msg->header.stamp; + prev_pos = pos; + vel = tf2::Vector3(0, 0, 0); + prev_diff = vel; + }else{ + auto _dt = (msg->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 = msg->header.stamp; + } + opti_vel = rot_offset*vel; + //////////////////////// + + opti_pos_port->write(vec); + opti_vel_port->write(Vector3D(opti_vel.x(), opti_vel.y(), opti_vel.z())); + opti_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)); @@ -178,6 +250,12 @@ void ROSUnit_PoseProvider::callback_angular_vel(const geometry_msgs::Vector3Stam imu_angular_rt_port->write(vec); } +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); +} + 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}; From 75fe85396dc18c03b388a7c98c3edaedf9d240fe Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Wed, 30 Aug 2023 17:30:04 +0400 Subject: [PATCH 34/36] standard trajectory added --- hear_ros.cmake | 32 +++++++++++++------ .../ROSUnit_PX4AngularVelocittyProvider.hpp | 5 ++- include/HEAR_ROS/ROSUnit_PoseProvider.hpp | 6 +++- src/ROSUnit_PoseProvider.cpp | 6 ++-- 4 files changed, 35 insertions(+), 14 deletions(-) diff --git a/hear_ros.cmake b/hear_ros.cmake index 49ca34b..fbe1acc 100755 --- a/hear_ros.cmake +++ b/hear_ros.cmake @@ -3,15 +3,27 @@ set(HEAR_ROS_SOURCE_DIR ${CMAKE_CURRENT_LIST_DIR}/src) file(GLOB HEAR_ROS_SRCs ${HEAR_ROS_SOURCE_DIR}/*.cpp) -find_package(catkin REQUIRED COMPONENTS - roscpp - hear_msgs - mavros_msgs - std_srvs - geometry_msgs - tf2_geometry_msgs - tf2 - tf2_ros -) +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_PX4AngularVelocittyProvider.hpp b/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp index 0e57bfd..fe0c1c8 100755 --- a/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PX4AngularVelocittyProvider.hpp @@ -1,5 +1,6 @@ #ifndef ROSUNIT_PX4ANGULARVELOCITYPROVIDER_HPP #define ROSUNIT_PX4ANGULARVELOCITYPROVIDER_HPP +#ifdef PX4 #include @@ -22,7 +23,9 @@ class ROSUnit_PX4AngularVelocittyProvider{ ros::ServiceServer m_server; ExternalOutputPort>* imu_angular_rt_port; + #ifdef PX4 void callback_angular_vel(const mavros_msgs::VehicleAngularVelocity::ConstPtr&); + #endif public: void process(){} @@ -32,5 +35,5 @@ class ROSUnit_PX4AngularVelocittyProvider{ }; } - +#endif #endif \ No newline at end of file diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp index 2feed88..9f7436e 100755 --- a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp @@ -4,7 +4,9 @@ #include #include "ros/ros.h" -#include +#ifdef PX4 + #include +#endif #include #include #include @@ -43,7 +45,9 @@ class ROSUnit_PoseProvider{ 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_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, rot_offset_vision; tf2::Vector3 trans_offset, trans_offset_vision; diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index 8f53332..a18119d 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -63,14 +63,14 @@ ExternalOutputPort>* ROSUnit_PoseProvider::registerImuAngularRat xsens_ang_vel_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_angular_vel, this, ros::TransportHints().tcpNoDelay()); return imu_angular_rt_port; } - +#ifdef PX4 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::registerImuAcceleration(std::string t_name){ imu_acc_port = new ExternalOutputPort>(); imu_acc_port->write(Vector3D(0,0,0)); @@ -250,11 +250,13 @@ void ROSUnit_PoseProvider::callback_angular_vel(const geometry_msgs::Vector3Stam imu_angular_rt_port->write(vec); } +#ifdef PX4 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}; From c6f94b9955da8b3dd9eedc5abac8e1daabc0e258 Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Thu, 31 Aug 2023 12:21:49 +0100 Subject: [PATCH 35/36] Add PX4 vel and ori --- include/HEAR_ROS/ROSUnit_PoseProvider.hpp | 19 +++- src/ROSUnit_PoseProvider.cpp | 132 ++++++++++++---------- 2 files changed, 85 insertions(+), 66 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp index 9f7436e..9c33c34 100755 --- a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp @@ -5,11 +5,13 @@ #include "ros/ros.h" #ifdef PX4 + #include #include #endif #include #include #include +#include #include #include #include @@ -26,12 +28,15 @@ namespace HEAR{ class ROSUnit_PoseProvider{ private: ros::NodeHandle nh_; - ros::Subscriber opti_sub, vision_sub, px4_ang_vel_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; @@ -41,15 +46,17 @@ class ROSUnit_PoseProvider{ 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_px4_pose(const geometry_msgs::PoseStamped::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, rot_offset_vision; + tf2::Matrix3x3 rot_offset, rot_offset_vision, rot_offset_PX4_ori; tf2::Vector3 trans_offset, trans_offset_vision; tf2::Vector3 opti_pos, prev_pos, opti_vel, prev_diff, _hold; @@ -65,10 +72,12 @@ class ROSUnit_PoseProvider{ ~ROSUnit_PoseProvider(){} std::vector>*> registerOptiPose(std::string t_name); std::vector>*> registerVisionPose(std::string t_name); - std::vector>*> registerPX4Pose(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>* registerPX4ImuAngularRate(std::string t_name); ExternalOutputPort>* registerImuAcceleration(std::string t_name); }; diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index a18119d..99013fd 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -35,21 +35,46 @@ std::vector>*> ROSUnit_PoseProvider::register return std::vector>*>{vision_pos_port, vision_vel_port, vision_ori_port}; } -std::vector>*> ROSUnit_PoseProvider::registerPX4Pose(std::string t_name){ +#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(); - opti_pos_port = new ExternalOutputPort>(); - opti_pos_port->write(Vector3D(0,0,0)); - opti_vel_port = new ExternalOutputPort>(); - opti_vel_port->write(Vector3D(0,0,0)); - opti_ori_port = new ExternalOutputPort>(); - opti_ori_port->write(Vector3D(0,0,0)); - opti_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_px4_pose, this, ros::TransportHints().tcpNoDelay()); - return std::vector>*>{opti_pos_port, opti_vel_port, opti_ori_port}; + 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); + + 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>(); imu_ori_port->write(Vector3D(0,0,0)); @@ -63,14 +88,6 @@ ExternalOutputPort>* ROSUnit_PoseProvider::registerImuAngularRat xsens_ang_vel_sub = nh_.subscribe(t_name, 10, &ROSUnit_PoseProvider::callback_angular_vel, this, ros::TransportHints().tcpNoDelay()); return imu_angular_rt_port; } -#ifdef PX4 -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::registerImuAcceleration(std::string t_name){ imu_acc_port = new ExternalOutputPort>(); imu_acc_port->write(Vector3D(0,0,0)); @@ -183,11 +200,30 @@ void ROSUnit_PoseProvider::callback_vision_pose(const geometry_msgs::PoseWithCov 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)); + + tf2Scalar yaw, roll, pitch; + R_mat.getEulerYPR(yaw, pitch, roll); + + Vector3D vec = {(float)roll, (float)pitch, (float)yaw}; + imu_ori_port->write(vec); +} + +void ROSUnit_PoseProvider::callback_angular_vel(const geometry_msgs::Vector3Stamped::ConstPtr& msg){ + Vector3D vec = {(float)msg->vector.x, (float)msg->vector.y, (float)msg->vector.z}; + + imu_angular_rt_port->write(vec); +} + +#ifdef PX4 void ROSUnit_PoseProvider::callback_px4_pose(const geometry_msgs::PoseStamped::ConstPtr& msg){ - tf2::Vector3 vel; auto pos = tf2::Vector3({msg->pose.position.x, msg->pose.position.y, msg->pose.position.z}); - auto calib_pos = pos; //rot_offset*pos - trans_offset; + auto calib_pos = rot_offset*pos - trans_offset; Vector3D vec = {(float)calib_pos.x(), (float)calib_pos.y(), (float)calib_pos.z()}; @@ -199,58 +235,32 @@ void ROSUnit_PoseProvider::callback_px4_pose(const geometry_msgs::PoseStamped::C Vector3D vec_ori = {(float)roll, (float)pitch, (float)yaw}; - // velocity calculation - if(first_read == 0){ - first_read = 1; - prevT = msg->header.stamp; - prev_pos = pos; - vel = tf2::Vector3(0, 0, 0); - prev_diff = vel; - }else{ - auto _dt = (msg->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 = msg->header.stamp; - } - opti_vel = rot_offset*vel; - //////////////////////// - opti_pos_port->write(vec); - opti_vel_port->write(Vector3D(opti_vel.x(), opti_vel.y(), opti_vel.z())); - opti_ori_port->write(vec_ori); + px4_pos_port->write(vec); } +void ROSUnit_PoseProvider::callback_px4_vel(const geometry_msgs::TwistStamped::ConstPtr& msg){ -void ROSUnit_PoseProvider::callback_ori(const geometry_msgs::QuaternionStamped::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->quaternion.x, msg->quaternion.y, msg->quaternion.z, msg->quaternion.w)); + auto R_mat = tf2::Matrix3x3(tf2::Quaternion(msg->q_x, msg->q_y, msg->q_z, msg->q_w )); - tf2Scalar yaw, roll, pitch; + R_mat = rot_offset_PX4_ori * R_mat * rot_offset_PX4_ori.transpose(); + tf2Scalar yaw, pitch, roll; R_mat.getEulerYPR(yaw, pitch, roll); - Vector3D vec = {(float)roll, (float)pitch, (float)yaw}; - imu_ori_port->write(vec); -} + Vector3D vec_ori = {(float)roll, (float)pitch, (float)yaw}; -void ROSUnit_PoseProvider::callback_angular_vel(const geometry_msgs::Vector3Stamped::ConstPtr& msg){ - Vector3D vec = {(float)msg->vector.x, (float)msg->vector.y, (float)msg->vector.z}; - imu_angular_rt_port->write(vec); + px4_ori_port->write(vec_ori); } - -#ifdef PX4 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}; From d4fc5855544c8005dcede09543237257affe490d Mon Sep 17 00:00:00 2001 From: HazemElrefaei Date: Thu, 31 Aug 2023 14:32:11 +0100 Subject: [PATCH 36/36] yaw angle corrected PX4 --- include/HEAR_ROS/ROSUnit_PoseProvider.hpp | 2 +- src/ROSUnit_PoseProvider.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp index 9c33c34..f385733 100755 --- a/include/HEAR_ROS/ROSUnit_PoseProvider.hpp +++ b/include/HEAR_ROS/ROSUnit_PoseProvider.hpp @@ -56,7 +56,7 @@ class ROSUnit_PoseProvider{ 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, rot_offset_vision, rot_offset_PX4_ori; + 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; diff --git a/src/ROSUnit_PoseProvider.cpp b/src/ROSUnit_PoseProvider.cpp index 99013fd..76862be 100755 --- a/src/ROSUnit_PoseProvider.cpp +++ b/src/ROSUnit_PoseProvider.cpp @@ -60,7 +60,8 @@ ExternalOutputPort>* ROSUnit_PoseProvider::registerPX4Vel(std::s } ExternalOutputPort>* ROSUnit_PoseProvider::registerPX4ImuOri(std::string t_name){ - rot_offset_PX4_ori.setRPY(M_PI, 0.0, 0.0); + 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)); @@ -252,7 +253,7 @@ void ROSUnit_PoseProvider::callback_px4_ori(const mavros_msgs::VehicleAttitude:: auto R_mat = tf2::Matrix3x3(tf2::Quaternion(msg->q_x, msg->q_y, msg->q_z, msg->q_w )); - R_mat = rot_offset_PX4_ori * R_mat * rot_offset_PX4_ori.transpose(); + 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);