added parameters to switch off mavlink raw publishing

.gitignore

@@ -1,4 +1,6 @@
 build
 bin
 msg_gen
-.DS_Store
+.DS_Store
+*.pyc
+

CMakeLists.txt

@@ -7,7 +7,7 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
 #  Release        : w/o debug symbols, w/ optimization
 #  RelWithDebInfo : w/ debug symbols, w/ optimization
 #  MinSizeRel     : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
+set(ROS_BUILD_TYPE RelWithDebInfo)
 
 ##########    CHOOSE PLATFORM   #############
 set(PLATFORM sensesoar)

include/params.h

@@ -0,0 +1,41 @@
+/*
+ * params.h
+ *
+ *  Created on: Aug 2, 2012
+ *      Author: slynen
+ */
+
+#ifndef PARAMS_H_
+#define PARAMS_H_
+
+#include <string.h>
+#include <ros/ros.h>
+#include <vector>
+#include <boost/algorithm/string.hpp>
+#include <iostream>
+#include <fstream>
+
+namespace mavlink_ros{
+
+class FixParams
+{
+public:
+	bool send_raw_mavlink;
+
+
+	FixParams(){
+		readParams();
+	}
+
+	void readParams(){
+
+		ros::NodeHandle nh("~");
+		nh.getParam("send_raw_mavlink", send_raw_mavlink);
+	}
+
+};
+
+}
+
+#endif /* PARAMS_H_ */
+

launch/mavlink_ros_bridge.launch

@@ -0,0 +1,6 @@
+<launch>
+    <node name="mavlink_ros_serial" pkg="mavlink_ros" type="mavlink_ros_serial" clear_params="true" output="screen">
+	<rosparam file="$(find mavlink_ros)/params/mavlink_ros_bridge.yaml"/>
+    </node>
+</launch>
+

params/mavlink_ros_bridge.yaml

@@ -0,0 +1 @@
+send_raw_mavlink: true

pymavlink/generator/mavgen_ros.py

@@ -94,9 +94,9 @@ class NodeHandleSingleton{
 {
     if(!publisher_${basename}_advertised){
      // Register all publishers   
-    ${{message:\tstd::cout<<"Registering publisher for ${name_lower}"<<std::endl;
+    ${{message:\tstd::cout<<"Registering publisher for ${name_lower}:";std::cout.flush();
 \tmavlink_ros_msg_${name_lower}_publisher = NodeHandleSingleton::Instance().advertise<mavlink_ros::mavlink_ros_msg_${name_lower}> ("/${basename}/${name_lower}", 1000);
-\tstd::cout<<"Registered publisher for ${name_lower}"<<std::endl;
+\tstd::cout<<"ok"<<std::endl;
     }}
     \tpublisher_${basename}_advertised = true;
     }

src/mavlink_ros_serial.cpp

@@ -35,6 +35,7 @@
 #include "ros/ros.h"
 
 #include "mavlink_ros/Mavlink.h"
+#include "params.h"
 
 #define PLATFORMINCLROSBRIDGE(x) <x/mavlink_ros_bridge.h>
 #include PLATFORMINCLROSBRIDGE(PLATFORM)
@@ -89,6 +90,9 @@ int fd;
 ros::Subscriber mavlink_sub;
 ros::Publisher mavlink_pub;
 
+/*some parameters*/
+mavlink_ros::FixParams* pfixparams = 0;
+
 /**
  *
  *
@@ -315,33 +319,34 @@ void* serial_wait(void* serial_ptr)
 			if (verbose || debug)
 				ROS_INFO("Received message from serial with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid);
 
-			/**
-			 * Serialize the Mavlink-ROS-message
-			 */
-			mavlink_ros::Mavlink rosmavlink_msg;
-
-			rosmavlink_msg.len = message.len;
-			rosmavlink_msg.seq = message.seq;
-			rosmavlink_msg.sysid = message.sysid;
-			rosmavlink_msg.compid = message.compid;
-			rosmavlink_msg.msgid = message.msgid;
-			rosmavlink_msg.fromlcm = false;
+			if(pfixparams->send_raw_mavlink){
+				/**
+				 * Serialize the Mavlink-ROS-message
+				 */
+				mavlink_ros::Mavlink rosmavlink_msg;
 
-			for (int i = 0; i < message.len/8; i++)
-			{
-				(rosmavlink_msg.payload64).push_back(message.payload64[i]);
-			}
+				rosmavlink_msg.len = message.len;
+				rosmavlink_msg.seq = message.seq;
+				rosmavlink_msg.sysid = message.sysid;
+				rosmavlink_msg.compid = message.compid;
+				rosmavlink_msg.msgid = message.msgid;
+				rosmavlink_msg.fromlcm = false;
 
-			/**
-			 * Mark the ROS-Message as coming not from LCM
-			 */
-			rosmavlink_msg.fromlcm = true;
+				for (int i = 0; i < message.len/8; i++)
+				{
+					(rosmavlink_msg.payload64).push_back(message.payload64[i]);
+				}
 
-			/**
-			 * Send the received MAVLink message to ROS (topic: mavlink, see main())
-			 */
-			mavlink_pub.publish(rosmavlink_msg);
+				/**
+				 * Mark the ROS-Message as coming not from LCM
+				 */
+				rosmavlink_msg.fromlcm = true;
 
+				/**
+				 * Send the received MAVLink message to ROS (topic: mavlink, see main())
+				 */
+				mavlink_pub.publish(rosmavlink_msg);
+			}
 			/**
 			 * Now decode the message to ros format and send
 			 */
@@ -393,6 +398,8 @@ void mavlinkCallback(const mavlink_ros::Mavlink &mavlink_ros_msg)
 int main(int argc, char **argv) {
 	ros::init(argc, argv, "mavlink_ros_serial");
 
+	pfixparams = new mavlink_ros::FixParams();
+
 	// Handling Program options
 	static GOptionEntry entries[] =
 	{
@@ -447,9 +454,11 @@ int main(int argc, char **argv) {
 
 
 	// SETUP ROS
-	ros::NodeHandle n;
-	mavlink_sub = n.subscribe("/toMAVLINK", 1000, mavlinkCallback);
-	mavlink_pub = n.advertise<mavlink_ros::Mavlink> ("/fromMAVLINK", 1000);
+	if(pfixparams->send_raw_mavlink){
+		ros::NodeHandle n;
+		mavlink_sub = n.subscribe("/toMAVLINK", 1000, mavlinkCallback);
+		mavlink_pub = n.advertise<mavlink_ros::Mavlink> ("/fromMAVLINK", 1000);
+	}
 	//	ros::NodeHandle attitude_nh;
 	//	attitude_pub = attitude_nh.advertise<sensor_msgs::Imu>("/fromMAVLINK/Imu", 1000);
 
@@ -503,6 +512,8 @@ int main(int argc, char **argv) {
 	//g_thread_join(serial_thread);
 	//exit(0);
 
+	delete pfixparams;
+
 	return 0;
 }