Merge pull request #69 from AutoModality/BB-886/collision_avoidance

feat: added new RC controller for logitech

Author: edkoch

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_;

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
     };

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]);
 	}
 }
 

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<sensor_msgs::Joy>("/joy", 10,
+			&RC_Controls::logitechRemoteCB, this);
+
 	dji_rc_channels_sub_ = n.subscribe<sensor_msgs::Joy>(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
 //===================================================

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],