From 2f5c0a432663242d15f20ac98e365dbdb8e8a56b Mon Sep 17 00:00:00 2001 From: edkoch Date: Thu, 14 Jan 2021 16:13:31 -0800 Subject: [PATCH 1/3] feat: added new RC controller for logitech --- include/vb_util_lib/rc_controls.h | 8 +++++ include/vb_util_lib/rc_mapper.h | 18 +++++++++++ src/vb_util_lib/rc_controls.cpp | 50 +++++++++++++++++++++++++++++++ src/vb_util_lib/rc_mapper.cpp | 20 ++++++++++++- 4 files changed, 95 insertions(+), 1 deletion(-) diff --git a/include/vb_util_lib/rc_controls.h b/include/vb_util_lib/rc_controls.h index f4a63dd..ce5d7dd 100644 --- a/include/vb_util_lib/rc_controls.h +++ b/include/vb_util_lib/rc_controls.h @@ -123,6 +123,14 @@ class RC_Controls bool setDJIGearSwitch(double gear_switch, RC_GEAR& state); void setDJIStickValue(double input, double& state) { state = input; } + // Logitech callback functions + void logitechRemoteCB(const sensor_msgs::Joy::ConstPtr& msg); + ros::Subscriber logitech_rc_channels_sub_; + void setLogitechControls(const sensor_msgs::Joy::ConstPtr& msg); + bool setLogitechModeSwitch(double mode_switch, RC_MODE& state); + bool setLogitechGearSwitch(double gear_switch, RC_GEAR& state); + void setLogitechStickValue(double input, double& state) { state = input; } + // PX4 callback functions void px4RemoteCB(const mavros_msgs::RCIn::ConstPtr& msg); ros::Subscriber px4_rc_channels_sub_; diff --git a/include/vb_util_lib/rc_mapper.h b/include/vb_util_lib/rc_mapper.h index 44cf834..17d645f 100644 --- a/include/vb_util_lib/rc_mapper.h +++ b/include/vb_util_lib/rc_mapper.h @@ -69,6 +69,23 @@ class rc_mapper #define ACSL_RC_MAX_VALUE 2020 #define ACSL_RC_MIN_VALUE 1020 + // LOGITECH + #define LOGITECH_RC_THROTTLE 1 + #define LOGITECH_RC_YAW 0 + #define LOGITECH_RC_ROLL 2 + #define LOGITECH_RC_PITCH 3 + #define LOGITECH_RC_GEAR 0 + #define LOGITECH_RC_MODE 0 + #define LOGITECH_RC_ARM 0 + + #define LOGITECH_RC_THROTTLE_SIGN 1 + #define LOGITECH_RC_YAW_SIGN -1 + #define LOGITECH_RC_ROLL_SIGN -1 + #define LOGITECH_RC_PITCH_SIGN 1 + + #define LOGITECH_RC_MAX_VALUE 1 + #define LOGITECH_RC_MIN_VALUE -1 + // Default #define DEFAULT_MIN_VALUE 1020 #define DEFAULT_MAX_VALUE 2020 @@ -99,6 +116,7 @@ class rc_mapper eRCT_PX4 = 0, eRCT_DJI, eRCT_ACSL, + eRCT_LOGITECH, eRCT_Unknown, eEndOfRCTypes }; diff --git a/src/vb_util_lib/rc_controls.cpp b/src/vb_util_lib/rc_controls.cpp index c70dbe5..1d69947 100644 --- a/src/vb_util_lib/rc_controls.cpp +++ b/src/vb_util_lib/rc_controls.cpp @@ -156,6 +156,9 @@ void RC_Controls::setSubscriber(ros::NodeHandle& n) { UPPER_VALUE_ = (MAX_VALUE_ - (RANGE_/2)); LOWER_VALUE_ = (MIN_VALUE_ + (RANGE_/2)); + logitech_rc_channels_sub_ = n.subscribe("/joy", 10, + &RC_Controls::logitechRemoteCB, this); + dji_rc_channels_sub_ = n.subscribe(am_topics::DJI_SDK_RC, 10, &RC_Controls::djiRemoteCB, this); @@ -267,6 +270,53 @@ void RC_Controls::djiRemoteCB(const sensor_msgs::Joy::ConstPtr& rc_msg) { } } +//=================================================== +// Logitech specific RC stuff +//=================================================== + +void RC_Controls::setLogitechControls(const sensor_msgs::Joy::ConstPtr& msg) { + + stamp = msg->header.stamp; + + throttle_ = msg->axes[throttle_idx_]; + yaw_ = msg->axes[yaw_idx_]; + pitch_ = msg->axes[pitch_idx_]; + roll_ = msg->axes[roll_idx_]; + gear_ = 0; + mode_ = 0; + + setLogitechStickValue(yaw_, yaw); + setLogitechStickValue(pitch_, pitch); + setLogitechStickValue(roll_, roll); + setLogitechStickValue(throttle_, throttle); + + printf("XXX RC: roll[%0.2f], pitch[%0.2f], yaw[%0.2f], throttle[%0.2f], gear[%0.2f], mode[%0.2f]\n", + roll_, pitch_, yaw_, throttle_, gear_, mode_); +} + +bool RC_Controls::setLogitechModeSwitch(double mode_switch, RC_MODE& state) { + bool change_state = false; + state = RC_MODE::POS; + + return change_state; +} + +bool RC_Controls::setLogitechGearSwitch(double gear_switch, RC_GEAR& state) { + bool change = false; + state = RC_GEAR::DOWN; + + return change; +} + +void RC_Controls::logitechRemoteCB(const sensor_msgs::Joy::ConstPtr& rc_msg) { + setLogitechControls(rc_msg); + + if (callback_ != NULL) + { + callback_(this); + } +} + //=================================================== // PX4 specific RC stuff //=================================================== diff --git a/src/vb_util_lib/rc_mapper.cpp b/src/vb_util_lib/rc_mapper.cpp index 8eb4966..e766bee 100644 --- a/src/vb_util_lib/rc_mapper.cpp +++ b/src/vb_util_lib/rc_mapper.cpp @@ -18,13 +18,14 @@ rc_mapper::rc_mapper(ros::NodeHandle n) m_RCTypeNames[eRCT_PX4].assign("PX4"); m_RCTypeNames[eRCT_DJI].assign("DJI"); m_RCTypeNames[eRCT_ACSL].assign("ACSL"); + m_RCTypeNames[eRCT_LOGITECH].assign("LOGITECH"); m_RCTypeNames[eRCT_Unknown].assign("Unknown"); //default settings m_RCType.assign(m_RCTypeNames[m_SetRC].c_str()); SetFromDefaults(); - //set parameters form the input parameters (if present) + //set parameters from the input parameters (if present) SetFromParameters(); } @@ -190,6 +191,23 @@ void rc_mapper::SetFromParameters(void) m_Handle.param("/RC/maxvalue", m_MaxValue, ACSL_RC_MAX_VALUE); break; } + case eRCT_LOGITECH: + { + m_Handle.param("/RC/throttle", m_Channels[eRCThrottle], LOGITECH_RC_THROTTLE); + m_Handle.param("/RC/yaw", m_Channels[eRCYaw], LOGITECH_RC_YAW); + m_Handle.param("/RC/roll", m_Channels[eRCRoll], LOGITECH_RC_ROLL); + m_Handle.param("/RC/pitch", m_Channels[eRCPitch], LOGITECH_RC_PITCH); + m_Handle.param("/RC/gear", m_Channels[eRCGear], LOGITECH_RC_GEAR); + m_Handle.param("/RC/mode", m_Channels[eRCMode], LOGITECH_RC_MODE); + m_Handle.param("/RC/arm", m_Channels[eRCArm], LOGITECH_RC_ARM); + m_Handle.param("/RC/throttle_sign", m_Signs[eThrottleSign], LOGITECH_RC_THROTTLE_SIGN); + m_Handle.param("/RC/yaw_sign", m_Signs[eYawSign], LOGITECH_RC_YAW_SIGN); + m_Handle.param("/RC/roll_sign", m_Signs[eRollSign], LOGITECH_RC_ROLL_SIGN); + m_Handle.param("/RC/pitch_sign", m_Signs[ePitchSign], LOGITECH_RC_PITCH_SIGN); + m_Handle.param("/RC/minvalue", m_MinValue, LOGITECH_RC_MIN_VALUE); + m_Handle.param("/RC/maxvalue", m_MaxValue, LOGITECH_RC_MAX_VALUE); + break; + } } ROS_INFO("RC: Parameter driven values: throttle[%d], yaw[%d], roll[%d], pitch[%d], gear[%d], mode[%d], arm[%d], min[%d], max[%d]", m_Channels[eRCThrottle], m_Channels[eRCYaw], m_Channels[eRCRoll], m_Channels[eRCPitch], From a360863ab347af85f1ff0bc27d21ec040907b2bc Mon Sep 17 00:00:00 2001 From: dan hennage Date: Thu, 14 Jan 2021 17:50:50 -0800 Subject: [PATCH 2/3] fix: logitech updates --- include/vb_util_lib/rc_mapper.h | 4 ++-- src/vb_util_lib/rc_controls.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/include/vb_util_lib/rc_mapper.h b/include/vb_util_lib/rc_mapper.h index 17d645f..7b1446a 100644 --- a/include/vb_util_lib/rc_mapper.h +++ b/include/vb_util_lib/rc_mapper.h @@ -79,8 +79,8 @@ class rc_mapper #define LOGITECH_RC_ARM 0 #define LOGITECH_RC_THROTTLE_SIGN 1 - #define LOGITECH_RC_YAW_SIGN -1 - #define LOGITECH_RC_ROLL_SIGN -1 + #define LOGITECH_RC_YAW_SIGN 1 + #define LOGITECH_RC_ROLL_SIGN 1 #define LOGITECH_RC_PITCH_SIGN 1 #define LOGITECH_RC_MAX_VALUE 1 diff --git a/src/vb_util_lib/rc_controls.cpp b/src/vb_util_lib/rc_controls.cpp index 1d69947..611d1dd 100644 --- a/src/vb_util_lib/rc_controls.cpp +++ b/src/vb_util_lib/rc_controls.cpp @@ -290,7 +290,7 @@ void RC_Controls::setLogitechControls(const sensor_msgs::Joy::ConstPtr& msg) { setLogitechStickValue(roll_, roll); setLogitechStickValue(throttle_, throttle); - printf("XXX RC: roll[%0.2f], pitch[%0.2f], yaw[%0.2f], throttle[%0.2f], gear[%0.2f], mode[%0.2f]\n", + ROS_INFO_THROTTLE(1.0, "XXX RC: roll[%0.2f], pitch[%0.2f], yaw[%0.2f], throttle[%0.2f], gear[%0.2f], mode[%0.2f]\n", roll_, pitch_, yaw_, throttle_, gear_, mode_); } From fc58ef0558edf041b4cc2642bc8052f72096759e Mon Sep 17 00:00:00 2001 From: edkoch Date: Tue, 19 Jan 2021 09:39:05 -0800 Subject: [PATCH 3/3] feat: Took out debug --- src/vb_util_lib/kinematics.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/vb_util_lib/kinematics.cpp b/src/vb_util_lib/kinematics.cpp index 2a61e98..c24b871 100644 --- a/src/vb_util_lib/kinematics.cpp +++ b/src/vb_util_lib/kinematics.cpp @@ -218,9 +218,9 @@ void Kinematics::setPosition(const ros::Time& update_time, double x, double y, d { Eigen::Vector3d vel_debug; getLinearVelocity(drone_ENU, vel_debug); - ROS_INFO("Kinematics Velocity standard[%s : %0.3f %0.3f, %0.3f] body[%s : %0.3f %0.3f, %0.3f]", - frame_id_.c_str(), velLinear_[0], velLinear_[1], velLinear_[2], - drone_ENU, vel_debug[0], vel_debug[1], vel_debug[2]); +// ROS_INFO("Kinematics Velocity standard[%s : %0.3f %0.3f, %0.3f] body[%s : %0.3f %0.3f, %0.3f]", +// frame_id_.c_str(), velLinear_[0], velLinear_[1], velLinear_[2], +// drone_ENU, vel_debug[0], vel_debug[1], vel_debug[2]); } }