Merge pull request #1 from Projet-Navigation-UPS/develop

Release TP recherche balle

recherche_balle_tp1/CMakeLists.txt

@@ -8,7 +8,12 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   sensor_msgs
   std_msgs
+  message_generation
   geometry_msgs
+  kobuki_msgs
+  cv_bridge
+  image_transport
+
 )
 
 ## System dependencies are found with CMake's conventions
@@ -45,11 +50,11 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
-#   FILES
-#   Message1.msg
+ add_message_files(
+   FILES
+   command.msg
 #   Message2.msg
-# )
+ )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -66,10 +71,11 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
-#   DEPENDENCIES
-#   sensor_msgs#   std_msgs
-# )
+ generate_messages(
+  DEPENDENCIES
+#   sensor_msgs
+   std_msgs
+ )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -103,7 +109,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
 #  INCLUDE_DIRS include
 #  LIBRARIES recherche_balle_tp1
-#  CATKIN_DEPENDS roscpp sensor_msgs std_msgs
+  CATKIN_DEPENDS message_runtime
 #  DEPENDS system_lib
 )
 
@@ -113,9 +119,10 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
+#include_directories(include)
 include_directories(
-  ${catkin_INCLUDE_DIRS}
+    include
+#  ${catkin_INCLUDE_DIRS}
 )
 
 ## Declare a C++ library
@@ -129,17 +136,36 @@ include_directories(
 # add_dependencies(recherche_balle_tp1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Declare a C++ executable
-add_executable(recherche_balle_tp1 src/main.cpp src/TurtleBot.cpp)
+
+
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+add_executable(bgr_to_rgb_converter_node src/bgrConverter_node.cpp )
+add_executable(ballSearch_node src/ballSearch_node.cpp src/BallSearch.cpp src/GraphicServer.cpp src/TurtleBotCamera.cpp src/analyse.cpp src/traitement.cpp)
+add_executable(command_node src/command_node.cpp src/TurtleBotCommand.cpp src/analyse.cpp src/traitement.cpp src/GraphicServer.cpp)
+
+
 
 ## Add cmake target dependencies of the executable
 ## same as for the library above
-add_dependencies(recherche_balle_tp1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(command_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(bgr_to_rgb_converter_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(ballSearch_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(recherche_balle_tp1
+
+target_link_libraries(ballSearch_node
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(command_node
+  ${catkin_LIBRARIES}
+)
+target_link_libraries(bgr_to_rgb_converter_node
   ${catkin_LIBRARIES}
 )
 
+
 #############
 ## Install ##
 #############

recherche_balle_tp1/include/BallSearch .hpp

@@ -0,0 +1,37 @@
+#ifndef _BALLSEARCH_
+#define _BALLSEARCH_
+
+#include <ros/ros.h>
+
+
+class BallSearch 
+{
+
+private:
+
+    //Subscrbers
+
+
+    //Publishers
+    ros::Publisher publisherBallReference;
+    ros::Publisher publisherBallReference2;
+
+    //Messages
+
+
+
+public:
+
+    BallSearch(ros::NodeHandle& node);
+    ~BallSearch();
+
+    void sendBallReference(const float linearVelocity, const float angularVelocity, const float distance, const float angle); 
+
+
+
+
+
+
+};
+
+#endif

recherche_balle_tp1/include/BallSearch.hpp

@@ -0,0 +1,43 @@
+#ifndef _BALLSEARCH_
+#define _BALLSEARCH_
+
+#include <ros/ros.h>
+
+#include "traitement.hpp"
+#include "analyse.hpp"
+#include "std_msgs/Bool.h"
+
+
+class BallSearch 
+{
+
+private:
+
+    //Subscrbers
+    ros::Subscriber subscriberCommandBusy;
+
+    //Publishers
+    ros::Publisher publisherBallReference;
+
+    //Messages
+    std_msgs::Bool command_busy;
+
+
+
+public:
+
+    BallSearch(ros::NodeHandle& node);
+    ~BallSearch();
+
+    void sendBallReference(const float linearVelocity, const float angularVelocity, const float distance, const float angle);
+    void callbackCommandBusy(const std_msgs::Bool& msg); 
+    Objet * Recherche_balle(unsigned char* raw, int  width, int height, int couleur);
+    void attente(int nsec, int sec);
+
+
+
+
+
+};
+
+#endif

recherche_balle_tp1/include/GraphicServer.hpp

@@ -0,0 +1,20 @@
+#ifndef _GRAPHICSERVER_
+#define _GRAPHICSERVER_
+
+#include <ros/ros.h>
+#include <string>
+#include "sensor_msgs/Image.h"
+
+class GraphicServer
+{
+
+public:
+    GraphicServer(ros::NodeHandle node,const std::string& topic);
+    void sendImageDisplay(sensor_msgs::Image imageMsg);
+
+private:    
+    ros::Publisher publisherImageDisplay;
+
+};
+
+#endif

recherche_balle_tp1/include/TurtleBot.hpp

@@ -0,0 +1,95 @@
+#ifndef _TURTLEBOT_
+#define _TURTLEBOT_
+
+#include <ros/ros.h>
+#include <string>
+#include <vector>
+#include "sensor_msgs/Image.h"
+#include "sensor_msgs/JointState.h"
+#include "geometry_msgs/Twist.h"
+#include "kobuki_msgs/Sound.h"
+
+//Constantes
+const int SOUND_ON = 0;
+const int SOUND_OFF = 1;
+const int SOUND_RECHARGE = 2;
+const int SOUND_BUTTON = 3;
+const int SOUND_ERROR = 4;
+const int SOUND_CLEANINGSTART = 5;
+const int SOUND_CLEANINGEND = 6;
+
+const float LINEAR_MAX_VELOCITY = 0.25;
+const float ANGULAR_MAX_VELOCITY = 2;
+
+const int CAMERA_WIDTH = 640;
+const int CAMERA_HEIGHT = 480;
+const int CAMERA_CHANNELS = 3;
+const int CAMERA_STEP_RGB = CAMERA_WIDTH * CAMERA_CHANNELS;
+const int CAMERA_STEP_MONO = CAMERA_WIDTH;
+
+const float ROBOT_MAX_LINEAR_VELOCITY = 0.5f;
+const float ROBOT_MAX_ANGULAR_VELOCITY = 3.14f;
+
+class TurtleBot 
+{
+
+private:
+
+    //Subscrbers
+    ros::Subscriber subscriberCameraRgbImageColor;
+    ros::Subscriber subscriberJointStates;
+
+    //Publishers
+    ros::Publisher publisherMobileBaseCommandsVelocity;
+    ros::Publisher publisherMobileBaseCommandsSound;
+
+    //Messages
+    sensor_msgs::Image cameraRgbImageColor;
+    unsigned char* cameraRgbImageColorRaw;
+    std::vector<unsigned char> cameraRgbImageColorVec;
+
+    kobuki_msgs::Sound mobileBaseCommandsSound;
+    geometry_msgs::Twist mobileBaseCommandsVelocity;
+    sensor_msgs::JointState JointStates;
+
+    ros::NodeHandle& m_node;
+public:
+
+    TurtleBot(ros::NodeHandle& node);
+    ~TurtleBot();
+
+    //Getters
+    sensor_msgs::Image getCameraRgbImageColor();
+    unsigned char* getCameraRgbImageColorRaw();
+
+    //Setters    
+    void setMobileBaseCommandsSound(const int sound);
+
+    //Image conversion
+    unsigned char* convertSensor_msgsImageToRaw(const sensor_msgs::Image& sensorMsgsImage);
+    sensor_msgs::Image convertRawToSensorMsgsImage(const unsigned char* raw, const int height, const int width, const std::string&encoding, const char is_bigendian, const int step);
+
+    //Displays
+    void displaySensorMsgsImage(const std::string& type, const sensor_msgs::Image& sensorMsgsImage);
+    void displayMobileBaseCommandsVelocity();
+    void displayMobileBaseCommandsSound();
+
+    //Motion
+    void stop();
+    void move(const float linearVelocity);
+    void turn(const float angularVelocity);
+
+    void moveAndTurn(const float linearVelocity, const float angularVelocity);
+
+private:
+    geometry_msgs::Twist getMobileBaseCommandsVelocity();
+    void setMobileBaseCommandsVelocity(const float linearX, const float linearY, const float linearZ, const float angularX, const float angularY, const float angularZ);
+
+    // simple callback to receive image from camera 
+    void callbackCameraRgbImageColor(const sensor_msgs::Image& msg);
+
+    void callbackJointStates(const sensor_msgs::JointState& msg);
+
+};
+
+#endif

recherche_balle_tp1/include/TurtleBotCamera.hpp

@@ -0,0 +1,74 @@
+#ifndef _TURTLEBOTCAMERA_
+#define _TURTLEBOTCAMERA_
+
+#include <ros/ros.h>
+#include <string>
+#include <vector>
+#include "sensor_msgs/Image.h"
+#include "kobuki_msgs/Sound.h"
+
+//Constantes
+const int SOUND_ON = 0;
+const int SOUND_OFF = 1;
+const int SOUND_RECHARGE = 2;
+const int SOUND_BUTTON = 3;
+const int SOUND_ERROR = 4;
+const int SOUND_CLEANINGSTART = 5;
+const int SOUND_CLEANINGEND = 6;
+
+
+const int CAMERA_WIDTH = 640;
+const int CAMERA_HEIGHT = 480;
+const int CAMERA_CHANNELS = 3;
+const int CAMERA_STEP_RGB = CAMERA_WIDTH * CAMERA_CHANNELS;
+const int CAMERA_STEP_MONO = CAMERA_WIDTH;
+
+
+class TurtleBotCamera
+{
+
+private:
+
+    //Subscrbers
+    ros::Subscriber subscriberCameraRgbImageColor;
+
+    //Publishers
+    ros::Publisher publisherMobileBaseCommandsSound;
+
+    //Messages
+    sensor_msgs::Image cameraRgbImageColor;
+    unsigned char* cameraRgbImageColorRaw;
+    std::vector<unsigned char> cameraRgbImageColorVec;
+
+    kobuki_msgs::Sound mobileBaseCommandsSound;
+
+    ros::NodeHandle& m_node;
+public:
+
+    TurtleBotCamera(ros::NodeHandle& node);
+    ~TurtleBotCamera();
+
+    //Getters
+    sensor_msgs::Image getCameraRgbImageColor();
+    unsigned char* getCameraRgbImageColorRaw();
+
+    //Setters    
+    void setMobileBaseCommandsSound(const int sound);
+
+    //Image conversion
+    unsigned char* convertSensor_msgsImageToRaw(const sensor_msgs::Image& sensorMsgsImage);
+    sensor_msgs::Image convertRawToSensorMsgsImage(const unsigned char* raw, const int height, const int width, const std::string&encoding, const char is_bigendian, const int step);
+
+    //Displays
+    void displaySensorMsgsImage(const std::string& type, const sensor_msgs::Image& sensorMsgsImage);
+    void displayMobileBaseCommandsSound();
+
+
+private:
+
+    // simple callback to receive image from camera 
+    void callbackCameraRgbImageColor(const sensor_msgs::Image& msg);
+
+};
+
+#endif