diff --git a/modules/catkin_ws/src/car/CMakeLists.txt b/modules/catkin_ws/src/car/CMakeLists.txt index 5633fb5ec625f552d4c2e022b0c04f9693b6c905..5cb136286cd0870a172d0bcfedab3fe3022c3184 100644 --- a/modules/catkin_ws/src/car/CMakeLists.txt +++ b/modules/catkin_ws/src/car/CMakeLists.txt @@ -22,6 +22,7 @@ add_message_files( rcEnabledMsg.msg stmDataMsg.msg ussDataMsg.msg + laneDataMsg.msg ) generate_messages( DEPENDENCIES @@ -58,6 +59,10 @@ add_library(camera src/camera/camera.cpp) add_dependencies(camera ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) target_link_libraries(camera ${catkin_LIBRARIES}) +add_library(lanekeeping src/lanekeeping/lanekeeping.cpp) +add_dependencies(lanekeeping ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_generate_messages_cpp) +target_link_libraries(lanekeeping ${catkin_LIBRARIES}) + install( TARGETS main_node diff --git a/modules/catkin_ws/src/car/include/lanekeeping/lanekeeping.h b/modules/catkin_ws/src/car/include/lanekeeping/lanekeeping.h new file mode 100644 index 0000000000000000000000000000000000000000..9446809bd234e8f7692655aaffceea9d54ee7c20 --- /dev/null +++ b/modules/catkin_ws/src/car/include/lanekeeping/lanekeeping.h @@ -0,0 +1,29 @@ +#ifndef LANEKEEPING_H +#define LANEKEEPING_H + +#include <nodelet/nodelet.h> +#include <ros/ros.h> + +#include "car/laneDataMsg.h" +#include "car/camDataMsg.h" + +namespace car +{ + class Lanekeeping : public nodelet::Nodelet + { + public: + virtual void onInit(); + Lanekeeping(ros::NodeHandle &nh, std::string &name); + Lanekeeping(); + ~Lanekeeping(); + private: + ros::NodeHandle nh_; + std::string name_; + + ros::Publisher laneData; + ros::Subscriber camData; + + void camDataCallback(const camDataMsg::ConstPtr& inMsg); + }; +} +#endif diff --git a/modules/catkin_ws/src/car/launch/fullstart.launch b/modules/catkin_ws/src/car/launch/fullstart.launch index a0971bf81285e51db6cbd9a1edede9b64120ef84..5c39d9d8d3041864d4de71eaaa57595cdc979557 100644 --- a/modules/catkin_ws/src/car/launch/fullstart.launch +++ b/modules/catkin_ws/src/car/launch/fullstart.launch @@ -7,6 +7,7 @@ <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"/> <node pkg="nodelet" type="nodelet" name="nodelet_camera" args="load car/camera Master"/> + <node pkg="nodelet" type="nodelet" name="nodelet_lanekeeping" args="load car/lanekeeping Master"/> </launch> <!-- diff --git a/modules/catkin_ws/src/car/msg/laneDataMsg.msg b/modules/catkin_ws/src/car/msg/laneDataMsg.msg new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml index 38af8257faf35a8b914d1930f4b3b1eac87cf404..6726b019505016846a9f355cb3aeb9c384e75928 100644 --- a/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml +++ b/modules/catkin_ws/src/car/plugins/nodelet_plugins.xml @@ -43,3 +43,13 @@ </description> </class> </library> + +<library path="lib/liblanekeeping"> + <class name="car/lanekeeping" + type="car::Lanekeeping" + base_class_type="nodelet::Nodelet"> + <description> + Missing + </description> + </class> +</library> diff --git a/modules/catkin_ws/src/car/src/lanekeeping/lanekeeping.cpp b/modules/catkin_ws/src/car/src/lanekeeping/lanekeeping.cpp new file mode 100644 index 0000000000000000000000000000000000000000..363f3d5456c2acc442589988aa52d760cd480c5b --- /dev/null +++ b/modules/catkin_ws/src/car/src/lanekeeping/lanekeeping.cpp @@ -0,0 +1,36 @@ +#include <pluginlib/class_list_macros.h> +#include <ros/ros.h> + +#include "lanekeeping/lanekeeping.h" + +#include "car/camDataMsg.h" +#include "car/environmentDataMsg.h" + +PLUGINLIB_EXPORT_CLASS(car::Lanekeeping, nodelet::Nodelet); + +namespace car +{ + Lanekeeping::Lanekeeping(ros::NodeHandle &nh, std::string &name) : + nh_(nh), + name_(name) + { } + + Lanekeeping::Lanekeeping() {} + + Lanekeeping::~Lanekeeping() {} + + void Lanekeeping::onInit() + { + NODELET_INFO("Lanekeeping::onInit -- START"); + laneData = nh_.advertise<laneDataMsg>("laneData", 1); + camData = nh_.subscribe("camData", 1, &Lanekeeping::camDataCallback, this); + NODELET_INFO("Lanekeeping::onInit -- END"); + } + + void Lanekeeping::camDataCallback(const camDataMsg::ConstPtr& inMsg) + { + std::cout << "Lanekeeping recived new cam data ( ).\n"; + } + + +}