lab

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) ...
+

autonomous_explorer/launch/start_exploration.launch

@@ -0,0 +1,4 @@
+<launch>
+  <node name="autonomous_exploration_node" pkg="autonomous_explorer" type="autonomous_exploration_node" output="screen" />
+</launch>
+

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
+

autonomous_explorer/package.xml

@@ -0,0 +1,46 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>autonomous_explorer</name>
+  <version>0.0.0</version>
+  <description>The autonomous_explorer package</description>
+
+  <!-- Maintainer details -->
+  <maintainer email="ros@todo.todo">ros</maintainer>
+
+  <!-- License information, update this as per your license details -->
+  <license>TODO</license>
+
+  <!-- Optional website URL -->
+  <!-- <url type="website">http://wiki.ros.org/autonomous_explorer</url> -->
+
+  <!-- Optional author details -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+  <!-- Package dependencies -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Replace rospy with roscpp for C++ implementation -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend> <!-- required if you're using custom messages -->
+
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>message_runtime</build_export_depend> <!-- required if you're using custom messages -->
+
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend> <!-- required if you're using custom messages -->
+
+  <!-- Export tag for additional information, can be left empty -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+  </export>
+</package>
+

autonomous_explorer/scripts/explorer.cpp

@@ -0,0 +1,85 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "sensor_msgs/LaserScan.h"
+#include <cstdlib> // For rand() and RAND_MAX
+#include <ctime> // For time()
+#include <vector>
+
+// Global variables
+std::vector<float> 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<geometry_msgs::Twist>("/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.
+}
+

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 <cstdlib> // For rand() and RAND_MAX
+#include <ctime> // For time()
+#include <vector>
+#include <cmath> // For M_PI and other mathematical operations
+#include "sensor_msgs/BatteryState.h"
+#include "nav_msgs/OccupancyGrid.h"
+
+// Global variables
+std::vector<float> 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<float>::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<geometry_msgs::Twist>("/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.
+}
+}
+