From 1b3012770cb9b84110b7983fb6790406f8ccf43b Mon Sep 17 00:00:00 2001
From: Hoop77 <p.badenhoop@gmx.de>
Date: Sun, 29 Apr 2018 21:38:24 +0200
Subject: [PATCH] mavlink node
---
modules/catkin_ws/src/car/CMakeLists.txt | 6 +-
.../src/car/include/mavLink/MavLink.h | 62 ++++++----
.../catkin_ws/src/car/src/mavLink/MavLink.cpp | 111 ++++++++++--------
3 files changed, 107 insertions(+), 72 deletions(-)
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index f1664b37..52becb3e 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/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
diff --git a/modules/catkin_ws/src/car/include/mavLink/MavLink.h b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
index 857c542d..4b0c17dd 100644
--- a/modules/catkin_ws/src/car/include/mavLink/MavLink.h
+++ b/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
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index dcd99e90..0d6b005d 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/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;
+}
+
+
}
--
GitLab