Add USS and ENV nodes + min Exampl

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}

modules/catkin_ws/src/car/include/environment/environment.h

+#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

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

modules/catkin_ws/src/car/include/ultrasonic/ultrasonic.h

+#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

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>
 
 <!--

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>

modules/catkin_ws/src/car/src/environment/environment.cpp

+#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);
+  }
+}

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";
+  }
 }

modules/catkin_ws/src/car/src/ultrasonic/ultrasonic.cpp

+#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");
+	}
+}