mavlink node

modules/catkin_ws/src/car/CMakeLists.txt

@@ -125,7 +125,11 @@ target_link_libraries(lanekeeping ${catkin_LIBRARIES})
 
 add_library(mav_link src/mavLink/MavLink.cpp)
 add_dependencies(mav_link ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
-target_link_libraries(mav_link ${catkin_LIBRARIES})
+target_link_libraries(mav_link
+        ${catkin_LIBRARIES}
+        VeloxProtocolLib)
+target_include_directories(mav_link PUBLIC
+        ${VeloxProtocolLib_INCLUDE_DIRS})
 
 # NotifiableThread Test
 set(NOTIFIABLE_THREAD_TEST_SOURCE_FILES

modules/catkin_ws/src/car/include/mavLink/MavLink.h

@@ -9,32 +9,46 @@
 #include "car/rcEnabledMsg.h"
 
 #include "boost/thread.hpp"
+#include "NetworkingLib/Networking.h"
+#include "VeloxProtocolLib/Connection.h"
 
 namespace car
 {
-	class MavLink : public nodelet::Nodelet
-	{
-		public:
-			virtual void onInit();
-			MavLink(ros::NodeHandle &nh, std::string &name);
-			MavLink();
-			~MavLink();
-		private:
-			ros::NodeHandle nh_;
-			std::string name_;
-      
-			ros::Publisher stmData;
-      ros::Subscriber ccData;
-      ros::Subscriber laneData;
-      ros::Subscriber rcEnabled;
-
-      
-      void ccDataCallback(const ccDataMsg::ConstPtr& inMsg);
-      void laneDataCallback(const laneDataMsg::ConstPtr& inMsg);
-      void rcEnabledCallback(const rcEnabledMsg::ConstPtr& inMsg);
-      
-      
-      boost::thread main;
-	};
+class MavLink : public nodelet::Nodelet
+{
+public:
+    virtual void onInit();
+
+    MavLink(ros::NodeHandle & nh, std::string & name);
+
+    MavLink();
+
+    ~MavLink();
+
+private:
+    ros::NodeHandle nh_;
+    std::string name_;
+
+    ros::Publisher stmData;
+    ros::Subscriber ccData;
+    ros::Subscriber laneData;
+    ros::Subscriber rcEnabled;
+
+    void ccDataCallback(const ccDataMsg::ConstPtr & inMsg);
+
+    void laneDataCallback(const laneDataMsg::ConstPtr & inMsg);
+
+    void rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg);
+
+    void onStmDataReceived();
+
+    float convertSpeedToStm(float speed);
+
+    float convertSpeedFromStm(float speed);
+
+    networking::Networking net;
+    veloxProtocol::Connection::Ptr veloxConnection;
+};
+
 }
 #endif

modules/catkin_ws/src/car/src/mavLink/MavLink.cpp

@@ -12,53 +12,70 @@ PLUGINLIB_EXPORT_CLASS(car::MavLink, nodelet::Nodelet);
 
 namespace car
 {
-	MavLink::MavLink(ros::NodeHandle &nh, std::string &name) :
-    nh_(nh), 
-    name_(name)
-    {}
-  
-	MavLink::MavLink() {}
-  
-	MavLink::~MavLink() {}
-  
-	void MavLink::onInit()
-	{
-		NODELET_INFO("MavLink::onInit -- START");
-	  stmData = nh_.advertise<stmDataMsg>("stmData", 1);
-	  ccData = nh_.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
-	  laneData = nh_.subscribe("laneData", 1, &MavLink::laneDataCallback, this);
-	  rcEnabled = nh_.subscribe("rcEnabled", 1, &MavLink::rcEnabledCallback, this);
-    main = boost::thread(
-        [this]()
-        {
-            int counter = 0;
-            ros::Rate rate{1};
-            while (ros::ok())
-            {
-                stmDataMsg msg;
-                msg.speed = counter++;
-                msg.angle = 0;
-                stmData.publish(msg);
-                rate.sleep();
-            }
-        });
-		NODELET_INFO("MavLink::onInit -- END");
-	}
-
-  void MavLink::ccDataCallback(const ccDataMsg::ConstPtr& inMsg)
-  {
-    std::cout << "MavLink recived new cc data (" << inMsg->speed << ").\n";
-  }
-
-  void MavLink::laneDataCallback(const laneDataMsg::ConstPtr& inMsg)
-  {
+MavLink::MavLink(ros::NodeHandle & nh, std::string & name) :
+    nh_(nh), name_(name)
+{}
+
+MavLink::MavLink()
+{}
+
+MavLink::~MavLink()
+{}
+
+void MavLink::onInit()
+{
+    NODELET_INFO("MavLink::onInit -- START");
+
+    stmData = nh_.advertise<stmDataMsg>("stmData", 1);
+    ccData = nh_.subscribe("ccData", 1, &MavLink::ccDataCallback, this);
+    laneData = nh_.subscribe("laneData", 1, &MavLink::laneDataCallback, this);
+    rcEnabled = nh_.subscribe("rcEnabled", 1, &MavLink::rcEnabledCallback, this);
+
+    veloxConnection = veloxProtocol::Connection::create(net);
+    veloxConnection->open(
+        "/dev/ttySAC0",
+        [this]
+        { onStmDataReceived(); },
+        [this]
+        { NODELET_INFO("Connection to STM closed!\n"); });
+
+    NODELET_INFO("MavLink::onInit -- END");
+}
+
+void MavLink::ccDataCallback(const ccDataMsg::ConstPtr & inMsg)
+{
+    veloxConnection->setSpeed(convertSpeedToStm(inMsg->speed));
+}
+
+void MavLink::laneDataCallback(const laneDataMsg::ConstPtr & inMsg)
+{
     std::cout << "MavLink recived new lane data ( ).\n";
-  }
-  
-  void MavLink::rcEnabledCallback(const rcEnabledMsg::ConstPtr& inMsg)
-  {
+}
+
+void MavLink::rcEnabledCallback(const rcEnabledMsg::ConstPtr & inMsg)
+{
     std::cout << "MavLink recived new rc status (" << inMsg->enabled << ").\n";
-  }
-  
-  
+}
+
+void MavLink::onStmDataReceived()
+{
+    stmDataMsg msg;
+    msg.speed = convertSpeedFromStm(veloxConnection->getMeasuredSpeed().get());
+    msg.angle = veloxConnection->getMeasuredSteeringAngle().get();
+    stmData.publish(msg);
+}
+
+float MavLink::convertSpeedToStm(float speed)
+{
+    // TODO: convert speed!
+    return speed;
+}
+
+float MavLink::convertSpeedFromStm(float speed)
+{
+    // TODO: convert speed!
+    return speed;
+}
+
+
 }