Merge pull request #10 from pge23-24/main

Final merge

launch_cario360.sh

@@ -0,0 +1,9 @@
+#!/bin/bash
+
+ABS_PATH=$(cd `dirname "${BASH_SOURCE[0]}"` && pwd)
+
+terminator --new-tab -x $ABS_PATH/vision_ws/launch_vision_yolo.sh && bash --rcfile <(echo ". ~/.bashrc && terminator --new-tab")
+terminator --new-tab -x $ABS_PATH/rob_ws/launch_rob_planner.sh && bash --rcfile <(echo ". ~/.bashrc && terminator --new-tab") &
+terminator --new-tab -x $ABS_PATH/rob_ws/launch_rob_ihm.sh && bash --rcfile <(echo ". ~/.bashrc && terminator --new-tab")  &     
+terminator --new-tab -x $ABS_PATH/rosbridge_ws/launch_bridge.sh && bash --rcfile <(echo ". ~/.bashrc && terminator --new-tab") &
+

rob_ws/build_rob.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+# Sourcing for ROS2 and ROS1
+source /opt/ros/noetic/setup.bash
+
+echo -e "\033[1;94m Building rob_ws... \033[0m"
+cd rob_ws && catkin build multi_obstacles_tracker_msgs
+catkin build
+echo -e "\033[1;94m Build rob_ws complete ! \033[0m"
+
+source rob_ws/devel/setup.bash

rob_ws/launch_rob_ihm.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+# source workspace
+source rob_ws/devel/setup.bash
+
+# ROS export
+export ROS_MASTER_URI=http://10.68.0.1:11311
+export ROS_IP=10.68.0.132
+
+echo -e "\033[1;94m Launch ihm agent... \033[0m"
+rosrun cario_ihm_agent relay_controller

rob_ws/launch_rob_planner.sh

@@ -0,0 +1,11 @@
+#!/bin/bash
+
+# source workspace
+source rob_ws/devel/setup.bash
+
+# ROS export
+export ROS_MASTER_URI=http://10.68.0.1:11311
+export ROS_IP=10.68.0.132
+
+echo -e "\033[1;94m Launch costmap inflater... \033[0m"
+roslaunch multi_obstacles_tracker local_costmap_inflater.launch

rob_ws/src/cario_ihm_agent/CMakeLists.txt

@@ -9,6 +9,7 @@ set(NODE_NAME relay_controller)
 find_package(catkin REQUIRED COMPONENTS 
 roscpp
 geometry_msgs
+nav_msgs
 rospy
 std_msgs
 multi_obstacles_tracker_msgs
@@ -25,13 +26,10 @@ catkin_package(
 
 include_directories(include ${catkin_INCLUDE_DIRS})
 
-add_executable(${NODE_NAME} src/relay_controller_node.cpp)
+add_executable(${NODE_NAME} src/robot_spot.cpp)
 
 # Move add_dependencies after add_executable
 add_dependencies(${NODE_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS}
                  ${catkin_EXPORTED_TARGETS})
 
-target_link_libraries(${NODE_NAME} ${catkin_LIBRARIES})
-
-set(PARENT_PROJECT_NAME ${PROJECT_NAME})
-add_subdirectory(test)
+target_link_libraries(${NODE_NAME} ${catkin_LIBRARIES})

rob_ws/src/cario_ihm_agent/include/robot_spot.hpp

@@ -0,0 +1,97 @@
+// Copyright 2024 cario360
+
+#ifndef ROB_WS_SRC_CARIO_IHM_AGENT_INCLUDE_RELAY_CONTROLLER_NODE_HPP_
+#define ROB_WS_SRC_CARIO_IHM_AGENT_INCLUDE_RELAY_CONTROLLER_NODE_HPP_
+
+#include <fcntl.h>
+#include <termios.h>
+#include <unistd.h>
+#include <mutex>
+
+#include "geometry_msgs/Twist.h"
+#include "nav_msgs/Odometry.h"
+#include "actionlib_msgs/GoalStatusArray.h"
+#include "move_base_msgs/RecoveryStatus.h"
+#include "multi_obstacles_tracker_msgs/CameraDetectionStampedArray.h"
+#include "multi_obstacles_tracker_msgs/CameraDetectionStamped.h"
+#include "ros/ros.h"
+#include <signal.h>
+
+extern int serial;
+
+#define PORT "/dev/ttyACM0"
+#define BAUDRATE B9600
+
+#define CMD_VEL_TOPIC "/mobile_base_controller/odom"
+#define JOY_TOPIC "/joy_priority_action/status"
+#define RECOVERY_TOPIC "/move_base/recovery_status"
+#define CAMERA_TOPIC "/camera_detection_1"
+
+#define SPOT_T_ON 0.25
+#define SPOT_T_OFF 0.25
+#define BUFFER_SIZE 10
+#define HALF_FOV_DEG 15
+
+//SPOT = Relay 1
+#define SPOT_ON "\x65"
+#define SPOT_OFF "\x6F"
+//GREEN = Relay 2
+#define MAT_GREEN_ON "\x66"
+#define MAT_GREEN_OFF "\x70"
+//ORANGE = Relay 3
+#define MAT_ORANGE_ON "\x67"
+#define MAT_ORANGE_OFF "\x71"
+//RED = Relay 4
+#define MAT_RED_ON "\x68"
+#define MAT_RED_OFF "\x72"
+
+#define ALL_OFF "\x6E"
+
+enum SpotState {ETEINT, CLIGNOTANT, ALLUME};
+enum BlinkState {B_ETEINT, B_WAIT1, B_ALLUME, B_WAIT2};
+enum MatState {IDLE_NAV, MANUAL, RECOVERY};
+enum SigJoy {UP, DOWN};
+
+
+
+class RelayControlNode {
+ public:
+  RelayControlNode(ros::NodeHandle nh, const char* port, int baudrate);
+  void cmd_vel_callback(const nav_msgs::Odometry& odom);
+  void joy_callback(const actionlib_msgs::GoalStatusArray& msg);
+  void recovery_callback(const move_base_msgs::RecoveryStatus& msg); //TODO: change the type of the message
+  // void timer_callback(const ros::TimerEvent& event);
+  void camera_detection_callback(const multi_obstacles_tracker_msgs::CameraDetectionStampedArray& msg);
+  void shutdown();
+  float compute_mean(float tab[]);
+  void init_buffer();
+  bool init_successful() { return init_success; }
+
+ private:
+  ros::Subscriber sub_cmd_vel, sub_joystick, sub_recovery, sub_camera_detection;
+  // ros::Timer timer;
+
+  // HARDWARE
+  int serial;
+  bool init_success;
+
+  // Déclaration du buffer circulaire pour les vitesses linéaires et angulaires (blue spot)
+  float linear_vel_buffer[BUFFER_SIZE];
+  float angular_vel_buffer[BUFFER_SIZE];
+  int current_index;
+  double timerStart;
+  std::mutex detection_mutex;
+  bool human_in_fov;
+  SpotState spot_state = ETEINT;
+  BlinkState blink_state = B_ETEINT;
+
+  // gyrophare
+  MatState mat_state = IDLE_NAV;
+  SigJoy sig_joy = DOWN;
+  ros::Time latest_stamp;
+  std::mutex recovery_mutex;
+  bool joy_actif = false;
+  bool recovery_actif = false;
+};
+
+#endif  // ROB_WS_SRC_CARIO_IHM_AGENT_INCLUDE_RELAY_CONTROLLER_NODE_HPP_

rob_ws/src/cario_ihm_agent/package.xml

@@ -34,6 +34,7 @@
   <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
   <depend>roscpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>multi_obstacle_tracker_msgs</depend>
 
 
@@ -46,6 +47,7 @@
   <test_depend>gtest</test_depend>
 
   <buildtool_depend>catkin</buildtool_depend>
+  <buildtool_depend>nav_msgs</buildtool_depend>
   <buildtool_depend>multi_obstacle_tracker_msgs</buildtool_depend>
 
   <!-- The export tag contains other, unspecified, tags -->