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..7b1446a 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/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]); } } diff --git a/src/vb_util_lib/rc_controls.cpp b/src/vb_util_lib/rc_controls.cpp index c70dbe5..611d1dd 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); + + 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_); +} + +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],