From 62f791dc9a4ab79bf9f187fb4d363811c29e73d6 Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Tue, 3 Apr 2018 17:28:31 +0200
Subject: [PATCH] First steps of EgoMotion
---
.../src/car/include/environment/Environment.h | 2 +-
.../src/car/include/mainNode/EgoMotion.h | 11 +++++++++
.../src/car/include/mavLink/MavLink.h | 5 ++++
.../src/car/msg/environmentDataMsg.msg | 2 +-
.../src/car/src/environment/Environment.cpp | 6 ++---
.../src/car/src/mainNode/EgoMotion.cpp | 23 ++++++++++++++++++-
.../catkin_ws/src/car/src/mavLink/MavLink.cpp | 14 +++++++++++
7 files changed, 57 insertions(+), 6 deletions(-)
diff --git a/modules/catkin_ws/src/car/include/environment/Environment.h b/modules/catkin_ws/src/car/include/environment/Environment.h
index 00f19700..ff28f961 100644
--- a/modules/catkin_ws/src/car/include/environment/Environment.h
+++ b/modules/catkin_ws/src/car/include/environment/Environment.h
@@ -25,7 +25,7 @@ private:
std::string name_;
float distance;
- float prevSpeed;
+ float relativeSpeed;
ros::Publisher environmentData;
ros::Subscriber ussData;
diff --git a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
index d398bd90..ea796eb2 100644
--- a/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
+++ b/modules/catkin_ws/src/car/include/mainNode/EgoMotion.h
@@ -5,8 +5,10 @@
#include <ros/ros.h>
#include <functional>
+#include <atomic>
#include "car/environmentDataMsg.h"
+#include "car/stmDataMsg.h"
namespace car
{
@@ -22,9 +24,18 @@ class EgoMotion
Callback cruiseControllerNotify;
private:
+ std::atomic<float> distance{0};
+ std::atomic<float> stmSpeed{0};
+ std::atomic<float> relSpeed{0};
+ std::atomic<float> prvSpeed{0};
+
ros::Subscriber environmentData;
+ ros::Subscriber stmData;
+
void environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg);
+ void stmDataCallback(const stmDataMsg::ConstPtr & inMsg);
+ void computeAndNotify();
};
}
diff --git a/modules/catkin_ws/src/car/include/mavLink/MavLink.h b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
index 836a23f0..857c542d 100644
--- a/modules/catkin_ws/src/car/include/mavLink/MavLink.h
+++ b/modules/catkin_ws/src/car/include/mavLink/MavLink.h
@@ -8,6 +8,8 @@
#include "car/laneDataMsg.h"
#include "car/rcEnabledMsg.h"
+#include "boost/thread.hpp"
+
namespace car
{
class MavLink : public nodelet::Nodelet
@@ -30,6 +32,9 @@ namespace car
void ccDataCallback(const ccDataMsg::ConstPtr& inMsg);
void laneDataCallback(const laneDataMsg::ConstPtr& inMsg);
void rcEnabledCallback(const rcEnabledMsg::ConstPtr& inMsg);
+
+
+ boost::thread main;
};
}
#endif
diff --git a/modules/catkin_ws/src/car/msg/environmentDataMsg.msg b/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
index 8fb4625f..080ad908 100644
--- a/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
+++ b/modules/catkin_ws/src/car/msg/environmentDataMsg.msg
@@ -1,2 +1,2 @@
float32 distance
-float32 prevSpeed
+float32 relativeSpeed
diff --git a/modules/catkin_ws/src/car/src/environment/Environment.cpp b/modules/catkin_ws/src/car/src/environment/Environment.cpp
index df596bbc..8718a215 100644
--- a/modules/catkin_ws/src/car/src/environment/Environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/Environment.cpp
@@ -15,7 +15,7 @@ namespace car
nh_(nh),
name_(name),
distance(0),
- prevSpeed(0) {}
+ relativeSpeed(0) {}
Environment::Environment() {}
@@ -34,11 +34,11 @@ namespace car
{
// std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
distance = inMsg->distance;
- prevSpeed = 1.0;
+ relativeSpeed = 1.0;
environmentDataMsg outMsg;
outMsg.distance = distance;
- outMsg.prevSpeed = prevSpeed;
+ outMsg.relativeSpeed = relativeSpeed;
environmentData.publish(outMsg);
}
diff --git a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
index 8ad351c0..261d606f 100644
--- a/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/EgoMotion.cpp
@@ -8,11 +8,32 @@ EgoMotion::EgoMotion(ros::NodeHandle nh)
, cruiseControllerNotify()
{
environmentData = nh.subscribe("environmentData", 1, &EgoMotion::environmentDataCallback, this);
+ stmData = nh.subscribe("stmData", 1, &EgoMotion::stmDataCallback, this);
}
void EgoMotion::environmentDataCallback(const environmentDataMsg::ConstPtr & inMsg)
{
- std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
+ std::cout << "EgoMotion recived new environmentData data (" << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << std::endl;
+ distance = inMsg->distance;
+ relSpeed = inMsg->relativeSpeed;
+ sleep(3);
+ computeAndNotify();
+ std::cout << "EgoMotion done with environmentData data (" << inMsg->distance << ", " << inMsg->relativeSpeed << ")." << std::endl;
+}
+
+
+void EgoMotion::stmDataCallback(const stmDataMsg::ConstPtr & inMsg)
+{
+ std::cout << "EgoMotion recived new stmData data (" << inMsg->speed << ", " << inMsg->angle << ")." << std::endl;
+ stmSpeed = inMsg->speed;
+ sleep(1);
+ computeAndNotify();
+ std::cout << "EgoMotion done with stmData data (" << inMsg->speed << ", " << inMsg->angle << ")." << std::endl;
+}
+
+void EgoMotion::computeAndNotify()
+{
+ prvSpeed = stmSpeed + relSpeed;
cruiseControllerNotify();
}
diff --git a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
index 6a735c5f..dcd99e90 100644
--- a/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
+++ b/modules/catkin_ws/src/car/src/mavLink/MavLink.cpp
@@ -28,6 +28,20 @@ namespace car
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");
}
--
GitLab