From 7672bcb1ef72d2e8d241d20781a828634d4cfb19 Mon Sep 17 00:00:00 2001
From: Franz Bethke <bethke@math.hu-berlin.de>
Date: Mon, 12 Feb 2018 12:21:55 +0100
Subject: [PATCH] Add USS and ENV nodes + min Exampl
---
modules/catkin_ws/src/car/CMakeLists.txt | 12 +++++
.../src/car/include/environment/environment.h | 32 +++++++++++++
.../src/car/include/mainNode/mainNode.h | 4 ++
.../src/car/include/ultrasonic/ultrasonic.h | 24 ++++++++++
.../catkin_ws/src/car/launch/fullstart.launch | 2 +
.../src/car/plugins/nodelet_plugins.xml | 20 ++++++++
.../src/car/src/environment/environment.cpp | 47 +++++++++++++++++++
.../src/car/src/mainNode/mainNode.cpp | 7 +++
.../src/car/src/ultrasonic/ultrasonic.cpp | 32 +++++++++++++
9 files changed, 180 insertions(+)
create mode 100644 modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h
create mode 100644 modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp
diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt
index 8aec3951..4bdfad45 100644
--- a/modules/catkin_ws/src/car/CMakeLists.txt
+++ b/modules/catkin_ws/src/car/CMakeLists.txt
@@ -31,6 +31,8 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES main_node
+ LIBRARIES environment
+ LIBRARIES ultrasonic
CATKIN_DEPENDS roscpp std_msgs nodelet rospy #message_runtime
)
@@ -43,9 +45,19 @@ add_library(main_node src/mainNode/mainNode.cpp)
add_dependencies(main_node ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(main_node ${catkin_LIBRARIES})
+add_library(environment src/environment/environment.cpp)
+add_dependencies(environment ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(environment ${catkin_LIBRARIES})
+
+add_library(ultrasonic src/ultrasonic/ultrasonic.cpp)
+add_dependencies(ultrasonic ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp)
+target_link_libraries(ultrasonic ${catkin_LIBRARIES})
+
install(
TARGETS
main_node
+ environment
+ ultrasonic
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
diff --git a/modules/catkin_ws/src/car/include/environment/environment.h b/modules/catkin_ws/src/car/include/environment/environment.h
index e69de29b..29d948e0 100644
--- a/modules/catkin_ws/src/car/include/environment/environment.h
+++ b/modules/catkin_ws/src/car/include/environment/environment.h
@@ -0,0 +1,32 @@
+#ifndef ENVIRONMENT_H
+#define ENVIRONMENT_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include "car/ussDataMsg.h"
+
+namespace car
+{
+ class Environment : public nodelet::Nodelet
+ {
+ public:
+ virtual void onInit();
+ Environment(ros::NodeHandle &nh, std::string &name);
+ Environment();
+ ~Environment();
+ private:
+ ros::NodeHandle nh_;
+ std::string name_;
+
+ float distance;
+ float prevSpeed;
+
+ ros::Publisher environmentData;
+ ros::Subscriber ussData;
+ ros::Subscriber camData;
+
+
+ void ussDataCallback(const ussDataMsg::ConstPtr& inMsg);
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/include/mainNode/mainNode.h b/modules/catkin_ws/src/car/include/mainNode/mainNode.h
index 46ab0157..1c3b73f2 100644
--- a/modules/catkin_ws/src/car/include/mainNode/mainNode.h
+++ b/modules/catkin_ws/src/car/include/mainNode/mainNode.h
@@ -4,6 +4,7 @@
#include <nodelet/nodelet.h>
#include <ros/ros.h>
#include "boost/thread.hpp"
+#include "car/environmentDataMsg.h"
namespace car
{
@@ -19,6 +20,9 @@ namespace car
std::string name_;
boost::thread loggerModule;
ros::Publisher logEnabled;
+ ros::Subscriber environmentData;
+
+ void environmentDataCallback(const environmentDataMsg::ConstPtr& inMsg);
};
}
#endif
diff --git a/modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h b/modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h
new file mode 100644
index 00000000..d31c30d3
--- /dev/null
+++ b/modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h
@@ -0,0 +1,24 @@
+#ifndef ULTRASONIC_H
+#define ULTRASONIC_H
+
+#include <nodelet/nodelet.h>
+#include <ros/ros.h>
+#include "boost/thread.hpp"
+
+namespace car
+{
+ class Ultrasonic : public nodelet::Nodelet
+ {
+ public:
+ virtual void onInit();
+ Ultrasonic(ros::NodeHandle &nh, std::string &name);
+ Ultrasonic();
+ ~Ultrasonic();
+ private:
+ ros::NodeHandle nh_;
+ std::string name_;
+ ros::Publisher ussData;
+ boost::thread main;
+ };
+}
+#endif
diff --git a/modules/catkin_ws/src/car/launch/fullstart.launch b/modules/catkin_ws/src/car/launch/fullstart.launch
index 068f3cd5..ec74c6bc 100644
--- a/modules/catkin_ws/src/car/launch/fullstart.launch
+++ b/modules/catkin_ws/src/car/launch/fullstart.launch
@@ -4,6 +4,8 @@
</node>
<node pkg="nodelet" type="nodelet" name="nodelet_main_node" args="load car/main_node Master"/>
+ <node pkg="nodelet" type="nodelet" name="nodelet_environment" args="load car/environment Master"/>
+ <node pkg="nodelet" type="nodelet" name="nodelet_ultrasonic" args="load car/ultrasonic Master"/>
</launch>
<!--
diff --git a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
index 6715ecbb..0767fe03 100644
--- a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
+++ b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml
@@ -13,3 +13,23 @@
</description>
</class>
</library>
+
+<library path="lib/libenvironment">
+ <class name="car/environment"
+ type="car::Environment"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Missing
+ </description>
+ </class>
+</library>
+
+<library path="lib/libultrasonic">
+ <class name="car/ultrasonic"
+ type="car::Ultrasonic"
+ base_class_type="nodelet::Nodelet">
+ <description>
+ Missing
+ </description>
+ </class>
+</library>
diff --git a/modules/catkin_ws/src/car/src/environment/environment.cpp b/modules/catkin_ws/src/car/src/environment/environment.cpp
index e69de29b..63934b43 100644
--- a/modules/catkin_ws/src/car/src/environment/environment.cpp
+++ b/modules/catkin_ws/src/car/src/environment/environment.cpp
@@ -0,0 +1,47 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "environment/environment.h"
+
+#include "car/camDataMsg.h"
+#include "car/environmentDataMsg.h"
+#include "car/ussDataMsg.h"
+
+PLUGINLIB_EXPORT_CLASS(car::Environment, nodelet::Nodelet);
+
+namespace car
+{
+ Environment::Environment(ros::NodeHandle &nh, std::string &name) :
+ nh_(nh),
+ name_(name),
+ distance(0),
+ prevSpeed(0) {}
+
+ Environment::Environment() {}
+
+ Environment::~Environment() {}
+
+ void Environment::onInit()
+ {
+ NODELET_INFO("Environment::onInit -- START");
+ environmentData = nh_.advertise<environmentDataMsg>("environmentData", 1);
+ ussData = nh_.subscribe("ussData", 1, &Environment::ussDataCallback, this);
+
+
+ // CODE GOES HERE
+ NODELET_INFO("Environment::onInit -- END");
+ }
+
+ void Environment::ussDataCallback(const ussDataMsg::ConstPtr& inMsg)
+ {
+ std::cout << "Environment recived new uss data (" << inMsg->distance << ").\n";
+ distance = inMsg->distance;
+ prevSpeed = 1.0;
+
+ environmentDataMsg outMsg;
+ outMsg.distance = distance;
+ outMsg.prevSpeed = prevSpeed;
+
+ environmentData.publish(outMsg);
+ }
+}
diff --git a/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp b/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
index 872ede66..ad5a8b1a 100644
--- a/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
+++ b/modules/catkin_ws/src/car/src/mainNode/mainNode.cpp
@@ -23,9 +23,16 @@ namespace car
{
NODELET_INFO("MainNode::onInit -- START");
logEnabled = nh_.advertise<logEnabledMsg>("logEnabled", 5);
+ environmentData = nh_.subscribe("environmentData", 1, &MainNode::environmentDataCallback, this);
+ std::cout << "MainNode &nh_: " << &nh_ << "\n";
// CODE GOES HERE
NODELET_INFO("MainNode::onInit -- END");
}
+
+ void MainNode::environmentDataCallback(const environmentDataMsg::ConstPtr& inMsg)
+ {
+ std::cout << "MainNode recived new environmentData data (" << inMsg->distance << ", " << inMsg->prevSpeed << ").\n";
+ }
}
diff --git a/modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp b/modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp
new file mode 100644
index 00000000..433b4c52
--- /dev/null
+++ b/modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp
@@ -0,0 +1,32 @@
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include "ultrasonic/ultrasonic.h"
+
+#include "car/ussDataMsg.h"
+
+PLUGINLIB_EXPORT_CLASS(car::Ultrasonic, nodelet::Nodelet);
+
+namespace car
+{
+ Ultrasonic::Ultrasonic(ros::NodeHandle &nh, std::string &name) : nh_(nh), name_(name) {}
+ Ultrasonic::Ultrasonic() {}
+ Ultrasonic::~Ultrasonic() {}
+ void Ultrasonic::onInit()
+ {
+ NODELET_INFO("Ultrasonic::onInit -- START");
+ ussData = nh_.advertise<ussDataMsg>("ussData", 1);
+ main = boost::thread([this] () {
+ int counter = 0;
+ ros::Rate rate{1};
+ while(ros::ok()) {
+ ussDataMsg msg;
+ msg.distance = counter++;
+ ussData.publish(msg);
+ rate.sleep();
+ }
+ });
+
+ NODELET_INFO("Ultrasonic::onInit -- END");
+ }
+}
--
GitLab