From 67bb0ebfeb83de18f2f4137fa059d25a18352a08 Mon Sep 17 00:00:00 2001 From: ros Date: Fri, 27 Oct 2023 12:07:41 +0200 Subject: [PATCH] lab --- autonomous_explorer/CMakeLists.txt | 107 ++++++++ .../launch/start_exploration.launch | 4 + autonomous_explorer/msg/Message1.msg | 17 ++ autonomous_explorer/package.xml | 46 ++++ autonomous_explorer/scripts/explorer.cpp | 85 +++++++ .../src/autonomous_explorer_node.cpp | 229 ++++++++++++++++++ 6 files changed, 488 insertions(+) create mode 100644 autonomous_explorer/CMakeLists.txt create mode 100644 autonomous_explorer/launch/start_exploration.launch create mode 100644 autonomous_explorer/msg/Message1.msg create mode 100644 autonomous_explorer/package.xml create mode 100755 autonomous_explorer/scripts/explorer.cpp create mode 100755 autonomous_explorer/src/autonomous_explorer_node.cpp diff --git a/autonomous_explorer/CMakeLists.txt b/autonomous_explorer/CMakeLists.txt new file mode 100644 index 0000000..b801291 --- /dev/null +++ b/autonomous_explorer/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.0.2) +project(autonomous_explorer) + +## Compile as C++11, supported in ROS Kinetic and newer +add_compile_options(-std=c++11) + +## Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + geometry_msgs + roscpp # Note: switch from rospy to roscpp for C++ + sensor_msgs + std_msgs + message_generation # for custom messages, services, and actions +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + +################################################ +## Declare ROS messages, services, and actions ## +################################################ + +# Note: If you don't have custom messages, services, or actions, +# you can remove the related declarations. + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# # ... (other message files) +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# # ... (other service files) +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# # ... (other action files) +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + geometry_msgs + sensor_msgs + std_msgs +) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + # INCLUDE_DIRS include + # LIBRARIES autonomous_explorer + CATKIN_DEPENDS geometry_msgs roscpp sensor_msgs std_msgs message_runtime + # DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +include_directories( + # include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ executable +add_executable(${PROJECT_NAME}_node src/autonomous_explorer_node.cpp) +add_executable(autonomous_exploration_node src/autonomous_explorer_node.cpp) +target_link_libraries(autonomous_exploration_node ${catkin_LIBRARIES}) # links the necessary libraries + +target_link_libraries(autonomous_exploration_node ${catkin_LIBRARIES}) + +## Add cmake target dependencies of the executable +## This will ensure that your messages, services or actions are processed before your executable is built +add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME}_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# All install targets should use catkin DESTINATION variables. +# See the catkin documentation for more info. + +# ... (installation-related commands, if any) ... + +############# +## Testing ## +############# + +# If you have tests for your package, place your commands here. + +# ... (test-related commands, if any) ... + diff --git a/autonomous_explorer/launch/start_exploration.launch b/autonomous_explorer/launch/start_exploration.launch new file mode 100644 index 0000000..c5f44b1 --- /dev/null +++ b/autonomous_explorer/launch/start_exploration.launch @@ -0,0 +1,4 @@ + + + + diff --git a/autonomous_explorer/msg/Message1.msg b/autonomous_explorer/msg/Message1.msg new file mode 100644 index 0000000..1d6ab67 --- /dev/null +++ b/autonomous_explorer/msg/Message1.msg @@ -0,0 +1,17 @@ +# Message1.msg + +# Standard header, which contains a timestamp. +std_msgs/Header header + +# The minimum distance reading from the laser scan, indicating how close the nearest obstacle is. +float32 min_distance_to_obstacle + +# Boolean indicating if an obstacle is within a critical range that requires stopping. +bool stopping_required + +# Current velocity of the robot as a safety check or for additional info. +geometry_msgs/Twist current_velocity + +# An optional field to provide any status messages or alerts. +string status + diff --git a/autonomous_explorer/package.xml b/autonomous_explorer/package.xml new file mode 100644 index 0000000..8f9d91b --- /dev/null +++ b/autonomous_explorer/package.xml @@ -0,0 +1,46 @@ + + + autonomous_explorer + 0.0.0 + The autonomous_explorer package + + + ros + + + TODO + + + + + + + + + catkin + + + roscpp + geometry_msgs + sensor_msgs + std_msgs + message_generation + + roscpp + geometry_msgs + sensor_msgs + std_msgs + message_runtime + + roscpp + geometry_msgs + sensor_msgs + std_msgs + message_runtime + + + + + + + diff --git a/autonomous_explorer/scripts/explorer.cpp b/autonomous_explorer/scripts/explorer.cpp new file mode 100755 index 0000000..32c735a --- /dev/null +++ b/autonomous_explorer/scripts/explorer.cpp @@ -0,0 +1,85 @@ +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/LaserScan.h" +#include // For rand() and RAND_MAX +#include // For time() +#include + +// Global variables +std::vector distance_data; +ros::Publisher pub; + +// Function declarations +void stop_robot(); +bool detect_obstacle(float range_val); + +void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) { + distance_data = msg->ranges; +} + +bool detect_obstacle(float range_val) { + for (float i : distance_data) { + if (i <= range_val) { + return true; // Obstacle detected + } + } + return false; // No obstacle +} + +void stop_robot() { + geometry_msgs::Twist vel_msg; + vel_msg.linear.x = 0.0; + vel_msg.angular.z = 0.0; + pub.publish(vel_msg); +} + +void move_forward() { + if (detect_obstacle(0.5)) { + stop_robot(); // Obstacle detected, stop the robot + } else { + geometry_msgs::Twist vel_msg; + vel_msg.linear.x = 0.5; + pub.publish(vel_msg); // No obstacle, move forward + } +} + +int main(int argc, char **argv) { + // Initialize the ROS system and become a node. + ros::init(argc, argv, "autonomous_exploration_node"); + ros::NodeHandle nh; + + // Create a publisher object. + pub = nh.advertise("/cmd_vel", 10); + + // Create a subscriber for the laser scanner data. + ros::Subscriber sub = nh.subscribe("/scan", 1000, laser_callback); + + // Seed the random number generator. + srand(time(0)); + + // Capture the start time to calculate elapsed time later. + ros::Time start_time = ros::Time::now(); + + // Define the operation duration. + ros::Duration duration(10.0); // 10 seconds + + ros::Rate rate(10); // 10 Hz + + // Main loop. + while (ros::ok()) { + // Decision-making logic to move or rotate. + move_forward(); + + // Check if 10 seconds have passed. + if (ros::Time::now() - start_time > duration) { + // If 10 seconds have passed, stop the robot and exit the loop. + stop_robot(); + break; + } + + ros::spinOnce(); // Call this function often to allow ROS to process incoming messages. + rate.sleep(); // Sleep for the rest of the cycle, to enforce the loop rate. + } + return 0; // Exit normally. +} + diff --git a/autonomous_explorer/src/autonomous_explorer_node.cpp b/autonomous_explorer/src/autonomous_explorer_node.cpp new file mode 100755 index 0000000..c88ada6 --- /dev/null +++ b/autonomous_explorer/src/autonomous_explorer_node.cpp @@ -0,0 +1,229 @@ +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include "sensor_msgs/LaserScan.h" +#include // For rand() and RAND_MAX +#include // For time() +#include +#include // For M_PI and other mathematical operations +#include "sensor_msgs/BatteryState.h" +#include "nav_msgs/OccupancyGrid.h" + +// Global variables +std::vector distance_data; +ros::Publisher pub; +float battery_percentage = 100.0; // Global variable to hold the latest battery percentage +nav_msgs::OccupancyGrid last_map; +bool last_map_initialized = false; +std::time_t last_update_time; + +// Function declarations +void stop_robot(); +bool detect_obstacle(float safe_distance); +void rotate_randomly(); + + +void batteryCallback(const sensor_msgs::BatteryState::ConstPtr& msg) { + battery_percentage = msg->percentage * 100; // Assuming that msg->percentage is between 0.0 and 1.0 + ROS_INFO("Battery: Current charge state is: %.2f%%", battery_percentage); +} + +void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) { + distance_data = msg->ranges; +} + +bool isMapSignificantlyChanged(const nav_msgs::OccupancyGrid& current_map) { + // Your criteria for significant change comes here. It might be based on + // the number of different cells, change in occupied space, etc. + // This is a placeholder for your actual comparison logic. + + // For simplicity, we're checking if the dimensions have changed. + return (current_map.info.width != last_map.info.width) || + (current_map.info.height != last_map.info.height) || + (current_map.data != last_map.data); // You might want to replace this with more specific logic +} + +void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& map) +{ + if (!last_map_initialized) { + last_map = *map; + last_map_initialized = true; + last_update_time = std::time(nullptr); + return; + } + + if (isMapSignificantlyChanged(*map)) { + last_update_time = std::time(nullptr); // reset the timer + last_map = *map; // update the saved map + } +} + +bool detect_obstacle(float safe_distance) { + // Safety check before processing the laser data + if (distance_data.empty()) { + ROS_WARN("Laser data is empty. No readings yet."); + return false; // Handle appropriately depending on your use-case + } + + // Diagnostic output of LIDAR readings + std::stringstream ss; + for (size_t i = 0; i < distance_data.size(); ++i) { + ss << " " << distance_data[i]; + if (i >= 10) { + ss << " ..."; // Indicate that there are more readings + break; + } + } + ROS_INFO_STREAM("LIDAR readings:" << ss.str()); + + // Assuming 360-degree coverage for the LIDAR, calculate indices for a 120-degree field + size_t total_readings = distance_data.size(); + size_t start_index = 360-90; // 120 degrees offset from the left + size_t end_index = 360; // -120 degrees offset from the right + + + // Check for obstacles within the specified field of view + bool valid_reading_found = false; + for (size_t i = start_index; i < end_index; ++i) { + float distance = distance_data[i]; + if (distance > std::numeric_limits::epsilon()) { + valid_reading_found = true; + if (distance < safe_distance) { + ROS_INFO_STREAM("Obstacle detected within 120-degree forward field. Distance: " << distance); + return true; // Obstacle detected within the safe distance + } + } + } + + if (!valid_reading_found) { + ROS_WARN("No valid LIDAR readings were found in the specified range. Possible sensor issue."); + } + + return false; // No obstacle within the safe distance or no valid readings in the specified range +} + + + + +void stop_robot() { + geometry_msgs::Twist vel_msg; + vel_msg.linear.x = 0.0; + vel_msg.angular.z = 0.0; + pub.publish(vel_msg); +} + +void rotate_randomly() { + // Generate a random number between 10 and 60 for the degrees of rotation +int random_degree = (rand() % 51) + 90; // Random degree between 60 and 90 + + // Convert degrees to radians for ROS + double random_radian = random_degree * M_PI / 180.0; + + // Construct a twist message for rotation + geometry_msgs::Twist vel_msg; + vel_msg.linear.x = 0.0; // No linear motion + + // Positive or negative rotation direction randomly + vel_msg.angular.z = random_radian * (rand() % 2 == 0 ? -1 : 1); + + // Publishing the message will cause the robot to rotate + pub.publish(vel_msg); + + // Assuming 1 second is enough for the required rotation + ros::Duration(1.0).sleep(); // Sleep for a second +} +/* +void move_forward() { + const float safe_distance = 0.15; // 15 cm safe distance + + if (detect_obstacle(safe_distance)) { + ROS_INFO("Obstacle detected. Rotating..."); + rotate_randomly(); // Obstacle detected, rotate randomly + } else { + geometry_msgs::Twist vel_msg; + vel_msg.angular.z=0.0; + vel_msg.linear.x = 0.3; // Move forward with a speed of 0.5 + pub.publish(vel_msg); // No obstacle, move forward + ROS_INFO("Moving forward."); + } +} +*/ + +void move_forward() { + const float safe_distance = 0.25; // Adjust the safe distance as needed + + static bool obstacle_detected = false; // Static variable to remember obstacle detection + + if (detect_obstacle(safe_distance)) { + if (!obstacle_detected) { + ROS_INFO("Obstacle detected. Rotating..."); + rotate_randomly(); + // obstacle_detected = true; // Set the flag to remember obstacle detection + } + } else { + obstacle_detected = false; // Reset the flag when no obstacle is detected + + geometry_msgs::Twist vel_msg; + vel_msg.angular.z = 0.0; // Make sure there's no angular velocity + vel_msg.linear.x = 0.3; // Move forward with a speed of 0.3 (adjust the speed as needed) + + pub.publish(vel_msg); // No obstacle, move forward + ROS_INFO("Moving forward."); + } +} +int main(int argc, char **argv) { + // Initialize the ROS system and become a node. + ros::init(argc, argv, "autonomous_exploration_node"); + ros::NodeHandle nh; + ros::Subscriber map_sub = nh.subscribe("/map", 10, mapCallback); + +ros::Subscriber battery_sub = nh.subscribe("/battery_state", 1000, batteryCallback); + + // Create a publisher object. + pub = nh.advertise("/cmd_vel", 10); + + // Create a subscriber for the laser scanner data. + ros::Subscriber sub = nh.subscribe("/scan", 1000, laser_callback); + + + // Seed the random number generator. + srand(time(0)); + + // Capture the start time to calculate elapsed time later. + ros::Time start_time = ros::Time::now(); + + // Define the operation duration. + ros::Duration duration(100.0); // 10 seconds + + ros::Rate rate(10); // 10 Hz + + // Main loop. +while (ros::ok()) { + // Decision-making logic to move or rotate. + move_forward(); + if (ros::Time::now() - last_update_time > 15) { + ROS_INFO("No significant map update for 15 seconds, stopping the robot."); + // Check if the battery is low. + if (battery_percentage <= 5.0) { + ROS_WARN("Battery is low. Stopping the robot."); + stop_robot(); + break; // Exit the loop + } + + // Check if 10 seconds have passed. + if (ros::Time::now() - start_time > duration) { + // If 10 seconds have passed, stop the robot and exit the loop. + ROS_INFO("10 seconds have passed. Stopping the robot."); + stop_robot(); + break; + } + + ros::spinOnce(); // Call this function often to allow ROS to process incoming messages. + rate.sleep(); // Sleep for the rest of the cycle, to enforce the loop rate. +} + + + ROS_INFO("Autonomous exploration node has been terminated."); + return 0; // Exit normally. +} +} +