Merge branch 'develop' of https://gitarero.ecam.fr/nathan.beaud/Dogbot into develop

RedBall_Chase.cpp

@@ -0,0 +1,152 @@
+#include "ros/ros.h"
+#include "sensor_msgs/Image.h"
+#include "geometry_msgs/Twist.h"
+#include "cv_bridge/cv_bridge.h"
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <iostream>
+#include <cmath>
+
+class RedBallChaser {
+public:
+    RedBallChaser() {
+        cmd_vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
+        image_sub = nh.subscribe("/camera/image", 1, &RedBallChaser::imageCallback, this);
+
+        cv::namedWindow("Red Ball Detection", cv::WINDOW_NORMAL);
+    }
+
+    ~RedBallChaser() {
+        cv::destroyAllWindows();
+    }
+
+    void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
+        cv_bridge::CvImagePtr cv_ptr;
+        try {
+            cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+        } catch (cv_bridge::Exception& e) {
+            ROS_ERROR("cv_bridge exception: %s", e.what());
+            return;
+        }
+
+        cv::Mat hsv_image;
+        cv::cvtColor(cv_ptr->image, hsv_image, cv::COLOR_BGR2HSV);
+
+        cv::Mat red_mask1, red_mask2, red_mask;
+        cv::inRange(hsv_image, cv::Scalar(0, 120, 70), cv::Scalar(10, 255, 255), red_mask1);
+        cv::inRange(hsv_image, cv::Scalar(170, 120, 70), cv::Scalar(180, 255, 255), red_mask2);
+        red_mask = red_mask1 | red_mask2;
+
+        std::vector<std::vector<cv::Point>> contours;
+        cv::findContours(red_mask, contours, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE);
+
+        bool ball_found = false;
+        for (auto &contour : contours) {
+            double area = cv::contourArea(contour);
+            if (area > 0) {
+                float radius;
+                cv::Point2f center;
+                cv::minEnclosingCircle(contour, center, radius);
+
+                if (radius > 5 && isCircular(contour, radius)) {
+                    ball_found = true;
+                    if (radius >= 40) {
+                        executeHalfCircle();
+                        return;
+                    }
+                    // Adjust robot's position and orientation
+                    adjustRobotPosition(center, cv_ptr->image.cols);
+                    break;
+                }
+            }
+        }
+
+        if (!ball_found) {
+            executeSearch();
+        }
+    }
+
+    void executeHalfCircle() {
+        geometry_msgs::Twist twist_msg;
+        twist_msg.angular.z = 0.5; // Adjust for half-circle
+        twist_msg.linear.x = -0.05; // Adjust for slight forward movement
+
+        ros::Time start = ros::Time::now();
+        double duration = 4.0; // Adjust for proper half-circle based on your robot
+        while (ros::Time::now() - start < ros::Duration(duration)) {
+            cmd_vel_pub.publish(twist_msg);
+            ros::Duration(0.1).sleep();
+        }
+
+        twist_msg.angular.z = 0;
+        twist_msg.linear.x = 0;
+        cmd_vel_pub.publish(twist_msg);
+    }
+
+    void executeSearch() {
+        rotate360();
+        moveRandomDirection();
+        rotate360();
+    }
+
+private:
+    ros::NodeHandle nh;
+    ros::Publisher cmd_vel_pub;
+    ros::Subscriber image_sub;
+
+    bool isCircular(std::vector<cv::Point> &contour, float radius) {
+        double area = cv::contourArea(contour);
+        double circularity = 4 * M_PI * area / (2 * M_PI * radius) * (2 * M_PI * radius);
+        return circularity >= 0.7 && circularity <= 1.3;
+    }
+
+    void adjustRobotPosition(cv::Point2f center, int frame_width) {
+        float err = center.x - frame_width / 2;
+        geometry_msgs::Twist twist_msg;
+
+        if (std::abs(err) > 100) {
+            twist_msg.angular.z = -0.005 * err;
+        } else {
+            twist_msg.linear.x = -0.2;
+        }
+        cmd_vel_pub.publish(twist_msg);
+    }
+
+    void rotate360() {
+        geometry_msgs::Twist twist_msg;
+        twist_msg.angular.z = 0.5; // Adjust as needed for the full rotation
+
+        ros::Time start = ros::Time::now();
+        while (ros::Time::now() - start < ros::Duration(2 * M_PI / twist_msg.angular.z)) {
+            cmd_vel_pub.publish(twist_msg);
+            ros::Duration(0.1).sleep();
+        }
+
+        twist_msg.angular.z = 0;
+        cmd_vel_pub.publish(twist_msg);
+    }
+
+    void moveRandomDirection() {
+        geometry_msgs::Twist twist_msg;
+        twist_msg.linear.x = -0.2; // Move forward
+
+        double move_duration = (rand() % 20 + 10) / 10.0; // Random duration between 1 and 3 seconds
+        ros::Time start = ros::Time::now();
+        while (ros::Time::now() - start < ros::Duration(move_duration)) {
+            cmd_vel_pub.publish(twist_msg);
+            ros::Duration(0.1).sleep();
+        }
+
+        twist_msg.linear.x = 0;
+        cmd_vel_pub.publish(twist_msg);
+    }
+};
+
+int main(int argc, char** argv) {
+    ros::init(argc, argv, "redball_chase_cpp");
+    RedBallChaser chaser;
+    ros::spin();
+    return 0;
+}
+

redball_chase.py

@@ -6,15 +6,16 @@ import numpy as np
 from sensor_msgs.msg import Image
 from geometry_msgs.msg import Twist
 from cv_bridge import CvBridge, CvBridgeError
-from sensor_msgs.msg import LaserScan
+import random
+import math
 
-# Define the target stopping distance from the ball in meters
+# Define the target stopping distance from the ball in pixels
-TARGET_DISTANCE = 1.0  
+RADIUS_THRESHOLD = 40  
 
 # Global variables for state management
 current_state = "searching"
 ball_position = None
-
+ball_radius = None
 
 def is_circular(contour, radius):
     """Check if the contour is circular enough to be considered a ball."""
@@ -41,21 +42,34 @@ def find_red_ball(image):
             return (int(x), int(y)), radius
     return None, None
 
-def lidar_callback(data):
+def execute_search():
-    """Process LiDAR data to adjust DogBot's behavior based on distance to obstacles."""
+    """Execute a search pattern: rotate 360, move randomly if the ball isn't found, and repeat."""
     global current_state
-    if current_state == "approaching":
+    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
-        # Assuming data.ranges is a list of distance measurements
+    twist_msg = Twist()
-        # You might need to process this data differently based on your LiDAR setup
+
-        distance_to_ball = min(data.ranges)
+    # Rotate 360 degrees
-        if distance_to_ball <= TARGET_DISTANCE:
+    twist_msg.angular.z = 0.5  # Adjust as needed
-            current_state = "maneuvering"
+    rotate_duration = 2 * math.pi / twist_msg.angular.z  # Full circle
-            # Placeholder for function to initiate maneuvering behind the ball
+    start_time = rospy.Time.now().to_sec()
-            execute_half_circle()
+    while rospy.Time.now().to_sec() - start_time < rotate_duration:
+        pub.publish(twist_msg)
+        rospy.sleep(0.1)
+    twist_msg.angular.z = 0
+    pub.publish(twist_msg)  # Stop rotation
+    
+    # Move in a random direction if the ball isn't found
+    if current_state == "searching":
+        twist_msg.linear.x = 0.2
+        move_duration = random.uniform(1, 3)  # Random duration between 1 and 3 seconds
+        start_time = rospy.Time.now().to_sec()
+        while rospy.Time.now().to_sec() - start_time < move_duration:
+            pub.publish(twist_msg)
+        twist_msg.linear.x = 0
+        pub.publish(twist_msg)  # Stop moving
 
 def image_callback(msg):
-    """Process image data to detect the red ball and control DogBot's movement."""
+    global current_state, ball_position, ball_radius
-    global current_state, ball_position
     try:
         cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
     except CvBridgeError as e:
@@ -63,55 +77,32 @@ def image_callback(msg):
 
     center, radius = find_red_ball(cv_image)
     ball_position = center
+    ball_radius = radius
 
-    twist_msg = Twist()
+    if center and radius >= RADIUS_THRESHOLD:
-    if center:
+        # Ball found and close enough, execute half-circle
-        x, _ = center
+        current_state = "maneuvering"
-        err = x - cv_image.shape[1] / 2
+        execute_half_circle()
-        if current_state == "searching" or current_state == "approaching":
+    elif current_state == "searching":
-            current_state = "approaching"
+        # Ball not found or not close enough, continue searching
-            if abs(err) > 100:  # Need to rotate DogBot to align with the ball
+        execute_search()
-                twist_msg.angular.z = -0.005 * err
-            else:  # Move forward towards the ball
-                twist_msg.linear.x = -0.2
-    else:
-        if current_state == "searching":
-            twist_msg.angular.z = 0.5  # Rotate DogBot if the ball is not found
-    cmd_vel_pub.publish(twist_msg)
 
 def execute_half_circle():
-    """Executes a half-circle maneuver to position the robot behind the ball."""
     global current_state
-    
     pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
     twist_msg = Twist()
-
+    twist_msg.angular.z = 0.5  # Adjust for half-circle
-    # Set angular velocity for a half-circle rotation
+    twist_msg.linear.x = 0.05  # Slight forward movement
-    twist_msg.angular.z = 0.5  # Positive for counter-clockwise, negative for clockwise
+    duration = 4  # Adjust for a proper half-circle based on your robot
-    
-    # Set a small or zero linear velocity to pivot around the ball
-    twist_msg.linear.x = -0.05
-
-    # Calculate duration needed to perform a half-circle maneuver around the ball
-    duration = 2  # Duration in seconds, adjust based on your robot's performance
-
-    # Start the maneuver
     start_time = rospy.Time.now().to_sec()
     while rospy.Time.now().to_sec() - start_time < duration:
         pub.publish(twist_msg)
-        rospy.sleep(0.1)  # Sleep briefly to allow message processing
-
-    # Stop the robot after completing the maneuver
     twist_msg.angular.z = 0
     twist_msg.linear.x = 0
-    pub.publish(twist_msg)
+    pub.publish(twist_msg)  # Stop
-
+    current_state = "approaching"
-    # Update the state to indicate the robot is now positioned behind the ball and ready to push it
-    current_state = "pushing"
-
 
 def shutdown():
-    """Ensure DogBot stops moving when the script is terminated."""
     rospy.loginfo("Shutting down")
     cmd_vel_pub.publish(Twist())
     cv2.destroyAllWindows()
@@ -121,7 +112,6 @@ if __name__ == '__main__':
     bridge = CvBridge()
     cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
     rospy.Subscriber("/camera/image", Image, image_callback)
-    rospy.Subscriber("/scan", LaserScan, lidar_callback)  # Subscribe to LiDAR data
 
     rospy.on_shutdown(shutdown)
     cv2.namedWindow("Red Ball Detection", cv2.WINDOW_NORMAL)
@@ -130,3 +120,4 @@ if __name__ == '__main__':
         rospy.spin()
     except KeyboardInterrupt:
         rospy.loginfo("Red ball follower node terminated.")
+