extras #387: initial import of servo_status_publisher

mavros_extras/CMakeLists.txt

@@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
   geometry_msgs
   std_msgs
   visualization_msgs
+  urdf
 )
 
 ## System dependencies are found with CMake's conventions
@@ -94,6 +95,13 @@ target_link_libraries(copter_visualization
   ${catkin_LIBRARIES}
 )
 
+add_executable(servo_state_publisher
+  src/servo_state_publisher.cpp
+)
+target_link_libraries(servo_state_publisher
+  ${catkin_LIBRARIES}
+)
+
 #############
 ## Install ##
 #############
@@ -110,7 +118,7 @@ install(PROGRAMS
 )
 
 ## Mark executables and/or libraries for installation
-install(TARGETS mavros_extras gcs_image_bridge copter_visualization
+install(TARGETS mavros_extras gcs_image_bridge copter_visualization servo_state_publisher
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

mavros_extras/launch/servo_state_publisher.yaml

@@ -0,0 +1,22 @@
+# Example config for servo_state_publisher
+# vim:set ts=2 sw=2 et:
+#
+aileron: &default
+  rc_channel: 0
+  rc_min: 1000  # for APM this values can be copied from RCx_MIN/MAX/TRIM
+  rc_max: 2000
+  rc_trim: 1500
+
+elevator:
+  <<: *default
+  rc_channel: 1
+
+# error if URDF don't define that <joint>
+throttle:
+  rc_channel: 2
+
+rudder:
+  rc_channel: 3
+  rc_min: 1100
+  rc_max: 1950  # rc_trim should calculate to 1525
+  rc_rev: true  # reverse

mavros_extras/package.xml

@@ -28,6 +28,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>visualization_msgs</depend>
+  <depend>urdf</depend>
 
   <export>
     <mavros plugin="${prefix}/mavros_plugins.xml" />

mavros_extras/src/servo_state_publisher.cpp

@@ -0,0 +1,118 @@
+/**
+ * @brief Publish servo states as JointState message
+ * @file
+ * @author Vladimir Ermakov <vooon341@gmail.com>
+ */
+/*
+ * Copyright 2015 Vladimir Ermakov
+ *
+ * This file is part of the mavros package and subject to the license terms
+ * in the top-level LICENSE file of the mavros repository.
+ * https://github.com/mavlink/mavros/tree/master/LICENSE.md
+ */
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <sensor_msgs/JointState.h>
+
+class ServoDescription {
+public:
+	std::string joint_name;
+	float joint_lower;
+	float joint_upper;
+
+	size_t rc_channel;
+
+	uint16_t rc_min;
+	uint16_t rc_max;
+	uint16_t rc_trim;
+	bool rc_rev;
+
+	ServoDescription() :
+		joint_name{},
+		joint_lower(-M_PI),
+		joint_upper(M_PI),
+		rc_channel(0),
+		rc_min(1000),
+		rc_max(2000),
+		rc_trim(1500),
+		rc_rev(false)
+	{ };
+
+	ServoDescription(std::string joint_name_, double lower_, double upper_,
+			int channel_,
+			int min_, int max_, int trim_,
+			bool rev_) :
+		joint_name(joint_name_),
+		joint_lower(lower_),
+		joint_upper(upper_),
+		rc_channel(channel_),
+		rc_min(min_),
+		rc_max(max_),
+		rc_trim(trim_),
+		rc_rev(rev_)
+	{ };
+};
+
+class ServoStatePublisher {
+public:
+	ServoStatePublisher() :
+		nh()
+	{
+		ros::NodeHandle priv_nh("~");
+
+		XmlRpc::XmlRpcValue param_dict;
+		priv_nh.getParam("", param_dict);
+
+		ROS_ASSERT(param_dict.getType() == XmlRpc::XmlRpcValue::TypeStruct);
+
+		for (auto &pair : param_dict) {
+			ROS_DEBUG("Loading joint: %s", pair.first.c_str());
+
+			// inefficient, but easier to program
+			ros::NodeHandle pnh(priv_nh, pair.first);
+
+			int rc_channel, rc_min, rc_max, rc_trim;
+			if (!pnh.getParam("rc_channel", rc_channel)) {
+				ROS_ERROR("Servo joint: %s should provice rc_channel", pair.first.c_str());
+				continue;
+			}
+
+			pnh.param("rc_min", rc_min, 1000);
+			pnh.param("rc_max", rc_max, 2000);
+			if (!pnh.getParam("rc_trim", rc_trim)) {
+				rc_trim = rc_min + (rc_max - rc_min) / 2;
+			}
+
+			bool rc_rev;
+			pnh.param("rc_rev", rc_rev, false);
+
+			// XXX TODO load from URDF lower/upper limits
+
+			servos.emplace_back(pair.first, 0.0, 0.0, rc_channel, rc_min, rc_max, rc_trim, rc_rev);
+			ROS_INFO("Servo joint: %s (RC%d) loaded", pair.first.c_str(), rc_channel);
+		}
+	}
+
+	void spin() {
+		ros::spin();
+	}
+
+private:
+	ros::NodeHandle nh;
+	ros::Subscriber rc_out_sub;
+	ros::Publisher joint_state_pub;
+
+	std::list<ServoDescription> servos;
+};
+
+int main(int argc, char *argv[])
+{
+	ros::init(argc, argv, "servo_state_publisher");
+
+	ServoStatePublisher state_publisher;
+	state_publisher.spin();
+	return 0;
+}
+