Merge pull request #4 from ethz-asl/devel/jay

Devel/jay

mavros/CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   sensor_msgs
   nav_msgs
   std_msgs
+  mav_msgs
   std_srvs
   tf2_ros
   angles

mavros/package.xml

@@ -50,6 +50,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>mav_msgs</depend>
 
   <test_depend>gtest</test_depend>
 

mavros/src/plugins/rc_io.cpp

@@ -17,7 +17,7 @@
 #include <mavros/mavros_plugin.h>
 #include <pluginlib/class_list_macros.h>
 
-#include <mavros_msgs/RCIn.h>
+#include <sensor_msgs/Joy.h>
 #include <mavros_msgs/RCOut.h>
 #include <mavros_msgs/OverrideRCIn.h>
 
@@ -27,6 +27,10 @@ namespace mavplugin {
  */
 class RCIOPlugin : public MavRosPlugin {
 public:
+
+	static constexpr double kStickMax = 2000;
+	static constexpr double kStickMin = 1000;
+
 	RCIOPlugin() :
 		rc_nh("~rc"),
 		uas(nullptr),
@@ -39,7 +43,7 @@ class RCIOPlugin : public MavRosPlugin {
 	{
 		uas = &uas_;
 
-		rc_in_pub = rc_nh.advertise<mavros_msgs::RCIn>("in", 10);
+		rc_in_pub = rc_nh.advertise<sensor_msgs::Joy>("in", 10);
 		rc_out_pub = rc_nh.advertise<mavros_msgs::RCOut>("out", 10);
 		override_sub = rc_nh.subscribe("override", 10, &RCIOPlugin::override_cb, this);
 
@@ -69,6 +73,40 @@ class RCIOPlugin : public MavRosPlugin {
 
 	/* -*- rx handlers -*- */
 
+	void send_rc_data(const ros::Time& stamp, const int rssi){
+		sensor_msgs::Joy rcin_msg;
+
+		rcin_msg.header.stamp = stamp;
+		//if rssi > 0, rc is on and so we set first button
+		rcin_msg.buttons.push_back(rssi > 0);
+
+		if(raw_rc_in.size() < 6){
+			ROS_FATAL("RC has too few control inputs and cannot be used, DO NOT FLY");
+		}
+		else{
+
+			std::vector<double> inputs;
+			for(const uint16_t& raw_rc_channel : raw_rc_in){
+			//convert to range -1 to 1
+				inputs.push_back((static_cast<double>(raw_rc_channel) - kStickMin) / (kStickMax - kStickMin));
+				inputs.back() = 2*inputs.back() - 1;
+				//ensure it is really in range
+				inputs.back() = std::min(std::max(inputs.back(),-1.0),1.0);
+			}
+
+			//weird ordering and inversions to match asctec interface
+			rcin_msg.axes.push_back(inputs[2]);
+			rcin_msg.axes.push_back(-inputs[1]);
+			rcin_msg.axes.push_back(inputs[0]);
+			rcin_msg.axes.push_back(-inputs[3]);
+			rcin_msg.axes.push_back(inputs[4]);
+			rcin_msg.axes.push_back(inputs[6]);
+			rcin_msg.axes.push_back(inputs[5]);
+
+			rc_in_pub.publish(rcin_msg);
+		}
+	}
+
 	void handle_rc_channels_raw(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
 		mavlink_rc_channels_raw_t port;
 		mavlink_msg_rc_channels_raw_decode(msg, &port);
@@ -94,13 +132,8 @@ class RCIOPlugin : public MavRosPlugin {
 		SET_RC_IN(8);
 #undef SET_RC_IN
 
-		auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
-
-		rcin_msg->header.stamp = uas->synchronise_stamp(port.time_boot_ms);
-		rcin_msg->rssi = port.rssi;
-		rcin_msg->channels = raw_rc_in;
-
-		rc_in_pub.publish(rcin_msg);
+		send_rc_data(uas->synchronise_stamp(port.time_boot_ms), port.rssi);
+
 	}
 
 	void handle_rc_channels(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
@@ -143,13 +176,7 @@ class RCIOPlugin : public MavRosPlugin {
 		IFSET_RC_IN(18);
 #undef IFSET_RC_IN
 
-		auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
-
-		rcin_msg->header.stamp = uas->synchronise_stamp(channels.time_boot_ms);
-		rcin_msg->rssi = channels.rssi;
-		rcin_msg->channels = raw_rc_in;
-
-		rc_in_pub.publish(rcin_msg);
+		send_rc_data(uas->synchronise_stamp(channels.time_boot_ms), channels.rssi);
 	}
 
 	void handle_servo_output_raw(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {

mavros/src/plugins/setpoint_attitude.cpp

@@ -61,11 +61,11 @@ class SetpointAttitudePlugin : public MavRosPlugin,
 			tf2_start("AttitudeSpTF", &SetpointAttitudePlugin::transform_cb);
 		}
 		else {
-			twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this);
-			pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this);
+			twist_sub = sp_nh.subscribe("cmd_vel", 10, &SetpointAttitudePlugin::twist_cb, this, ros::TransportHints().tcpNoDelay());
+			pose_sub = sp_nh.subscribe("attitude", 10, &SetpointAttitudePlugin::pose_cb, this, ros::TransportHints().tcpNoDelay());
 		}
 
-		throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this);
+		throttle_sub = sp_nh.subscribe("att_throttle", 10, &SetpointAttitudePlugin::throttle_cb, this, ros::TransportHints().tcpNoDelay());
 	}
 
 	const message_map get_rx_handlers() {

mavros/src/plugins/setpoint_raw.cpp

@@ -18,6 +18,8 @@
 #include <mavros/setpoint_mixin.h>
 #include <pluginlib/class_list_macros.h>
 #include <eigen_conversions/eigen_msg.h>
+#include <mav_msgs/RollPitchYawrateThrust.h>
+#include <tf/transform_datatypes.h>
 
 #include <mavros_msgs/AttitudeTarget.h>
 #include <mavros_msgs/PositionTarget.h>
@@ -44,9 +46,17 @@ class SetpointRawPlugin : public MavRosPlugin,
 
 		uas = &uas_;
 
+		if(!sp_nh.getParam("thrust_scaling_factor", thrust_scaling_)){
+			ROS_FATAL("No thrust scaling factor found, DO NOT FLY");
+		}
+    if(!sp_nh.getParam("yaw_rate_scaling_factor", yaw_rate_scaling_)){
+      ROS_FATAL("No yaw rate scaling factor found, DO NOT FLY");
+    }
+
 		local_sub = sp_nh.subscribe("local", 10, &SetpointRawPlugin::local_cb, this);
 		global_sub = sp_nh.subscribe("global", 10, &SetpointRawPlugin::global_cb, this);
 		attitude_sub = sp_nh.subscribe("attitude", 10, &SetpointRawPlugin::attitude_cb, this);
+		rpyt_sub = sp_nh.subscribe("roll_pitch_yawrate_thrust", 10, &SetpointRawPlugin::rpyt_cb, this);
 		target_local_pub = sp_nh.advertise<mavros_msgs::PositionTarget>("target_local", 10);
 		target_global_pub = sp_nh.advertise<mavros_msgs::GlobalPositionTarget>("target_global", 10);
 		target_attitude_pub = sp_nh.advertise<mavros_msgs::AttitudeTarget>("target_attitude", 10);
@@ -65,8 +75,9 @@ class SetpointRawPlugin : public MavRosPlugin,
 	ros::NodeHandle sp_nh;
 	UAS *uas;
 
-	ros::Subscriber local_sub, global_sub, attitude_sub;
+	ros::Subscriber local_sub, global_sub, attitude_sub, rpyt_sub;
 	ros::Publisher target_local_pub, target_global_pub, target_attitude_pub;
+	double thrust_scaling_, yaw_rate_scaling_;
 
 	/* -*- message handlers -*- */
 	void handle_position_target_local_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
@@ -124,7 +135,7 @@ class SetpointRawPlugin : public MavRosPlugin,
 		mavlink_attitude_target_t tgt;
 		mavlink_msg_attitude_target_decode(msg, &tgt);
 
-		// Transform orientation from baselink -> ENU 
+		// Transform orientation from baselink -> ENU
 		// to aircraft -> NED
 		auto orientation = UAS::transform_orientation_ned_enu(
 						   UAS::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tgt.q[0], tgt.q[1], tgt.q[2], tgt.q[3])));
@@ -256,6 +267,38 @@ class SetpointRawPlugin : public MavRosPlugin,
 				body_rate,
 				req->thrust);
 	}
+
+	void rpyt_cb(const mav_msgs::RollPitchYawrateThrustConstPtr msg) {
+
+		//the masks are much more limited than the docs would suggest so we don't use them
+  	uint8_t type_mask = 0;
+
+  	geometry_msgs::Quaternion orientation = tf::createQuaternionMsgFromRollPitchYaw(
+      msg->roll, msg->pitch, 0);
+
+		double thrust = std::min(1.0, std::max(0.0, msg->thrust.z * thrust_scaling_));
+
+		Eigen::Quaterniond desired_orientation;
+		Eigen::Vector3d body_rate;
+
+		tf::quaternionMsgToEigen(orientation, desired_orientation);
+
+		// Transform desired orientation to represent aircraft->NED,
+		// MAVROS operates on orientation of base_link->ENU
+		auto ned_desired_orientation = UAS::transform_orientation_enu_ned(
+			UAS::transform_orientation_baselink_aircraft(desired_orientation));
+
+    body_rate.x() = 0;
+    body_rate.y() = 0;
+		body_rate.z() = -yaw_rate_scaling_*msg->yaw_rate;
+
+		set_attitude_target(
+				msg->header.stamp.toNSec() / 1000000,
+				type_mask,
+				ned_desired_orientation,
+				body_rate,
+				thrust);
+	}
 };
 };	// namespace mavplugin