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;
+}
+

barking.py

@@ -0,0 +1,22 @@
+import pygame
+import time
+import random
+
+# Initialize pygame mixer
+pygame.mixer.init()
+
+# Load the sound file
+sound = pygame.mixer.Sound("bark5.wav")
+
+try:
+    while True:
+        # Play the sound
+        sound.play()
+
+        # Wait for the sound to finish playing
+        time.sleep(sound.get_length())
+
+        # Wait for a random interval between plays, e.g., 2 to 5 seconds
+        time.sleep(random.randint(2, 5))
+except KeyboardInterrupt:
+    print("Playback stopped by user.")

barking_and_chasing.launch

@@ -0,0 +1,7 @@
+<launch>
+    <!-- Launch barking.py -->
+    <node name="barking" pkg="my_robot_package" type="barking.py" output="screen"/>
+
+    <!-- Launch bring_ball.py -->
+    <node name="bring_ball" pkg="my_robot_package" type="bring_ball.py" output="screen"/>
+</launch>

dogbot_bark/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(dogbot_bark)
+
+## Find catkin and any catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  rospy
+  std_msgs
+)
+
+## Declare a catkin package
+catkin_package()
+
+## Mark executable scripts (Python etc.) for installation
+## This ensures the node can be run from anywhere and not just the source directory
+catkin_install_python(PROGRAMS scripts/bark.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(DIRECTORY src/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/src
+  FILES_MATCHING PATTERN "*.wav"
+)
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+

dogbot_bark/bark.py

@@ -0,0 +1,23 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+import os
+import rospkg
+
+def callback(data):
+    rospy.loginfo("Received a command to bark")
+    
+    # Get the path of the 'dogbot_bark' package
+    rospack = rospkg.RosPack()
+    package_path = rospack.get_path('dogbot_bark')
+    sound_file_path = os.path.join(package_path, 'src', 'Dog Woof.wav')
+    
+    os.system(f'aplay {sound_file_path}')  # Play the sound file
+
+def listener():
+    rospy.init_node('bark_listener', anonymous=True)
+    rospy.Subscriber("bark_command", String, callback)
+    rospy.spin()
+
+if __name__ == '__main__':
+    listener()

dogbot_bark/package.xml

@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>dogbot_bark</name>
+  <version>0.0.0</version>
+  <description>The dogbot_bark package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="dy11@todo.todo">dy11</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/dogbot_bark</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

owner_following.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+
+import rospy
+import cv2
+import numpy as np
+from pyzbar.pyzbar import decode
+from sensor_msgs.msg import Image, LaserScan
+from geometry_msgs.msg import Twist
+from cv_bridge import CvBridge, CvBridgeError
+import random
+
+TARGET_DISTANCE = 0.2  # Meters
+ANGULAR_SPEED = 0.5
+LINEAR_SPEED = 0.2
+
+class QRCodeFollower:
+    def __init__(self):
+        rospy.init_node('qr_code_follower', anonymous=False)
+        self.bridge = CvBridge()
+        self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+        self.image_sub = rospy.Subscriber("/camera/image", Image, self.image_callback)
+        self.laser_sub = rospy.Subscriber("/scan", LaserScan, self.laser_callback)
+        
+        self.qr_code_found = False
+        self.distance_to_owner = None
+
+    def rotate_360(self):
+        twist = Twist()
+        twist.angular.z = ANGULAR_SPEED
+        duration = 7.85 / ANGULAR_SPEED  # Time to complete a full rotation
+        start_time = rospy.Time.now().to_sec()
+        while rospy.Time.now().to_sec() - start_time < duration:
+            self.cmd_vel_pub.publish(twist)
+        twist.angular.z = 0
+        self.cmd_vel_pub.publish(twist)
+
+    def move_random_direction(self):
+        twist = Twist()
+        twist.linear.x = LINEAR_SPEED
+        duration = random.uniform(1, 3)  # Move for a random duration
+        start_time = rospy.Time.now().to_sec()
+        while rospy.Time.now().to_sec() - start_time < duration:
+            self.cmd_vel_pub.publish(twist)
+        twist.linear.x = 0
+        self.cmd_vel_pub.publish(twist)
+
+    def detect_qr_code(self, image):
+        decoded_objects = decode(image)
+        for obj in decoded_objects:
+            if obj.data.decode() == "Owner":
+                self.qr_code_found = True
+                
+                points = obj.polygon
+                if len(points) > 4: 
+                    hull = cv2.convexHull(np.array([point for point in points], dtype=np.float32))
+                    points = hull
+                n = len(points)
+                for j in range(n):
+                    cv2.line(image, points[j], points[(j+1) % n], (255,0,0), 3)
+                
+                cv2.imshow("QR Code Detection", image)
+                cv2.waitKey(1)
+                return True
+        return False
+
+    def image_callback(self, msg):
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+        except CvBridgeError as e:
+            rospy.logerr(e)
+            return
+        
+        if not self.detect_qr_code(cv_image) and not self.qr_code_found:
+            self.rotate_360()
+            self.move_random_direction()
+            self.rotate_360()
+
+    def laser_callback(self, msg):
+        self.distance_to_owner = min(msg.ranges)
+
+    def follow_qr_code(self):
+        twist = Twist()
+        if self.qr_code_found and self.distance_to_owner > TARGET_DISTANCE:
+            twist.linear.x = LINEAR_SPEED
+            rospy.loginfo(f"Distance to owner: {self.distance_to_owner:.2f} meters")
+        else:
+            twist.linear.x = 0
+            rospy.loginfo("Reached the target distance to owner.")
+        self.cmd_vel_pub.publish(twist)
+
+    def run(self):
+        rate = rospy.Rate(10)  # 10 Hz
+        while not rospy.is_shutdown():
+            if self.qr_code_found:
+                self.follow_qr_code()
+            rate.sleep()
+
+    def shutdown(self):
+        rospy.loginfo("Stopping QRCodeFollower...")
+        self.cmd_vel_pub.publish(Twist())
+        cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    qr_follower = QRCodeFollower()
+    rospy.on_shutdown(qr_follower.shutdown)
+    try:
+        qr_follower.run()
+    except rospy.ROSInterruptException:
+        pass

redball_chase.py

@@ -0,0 +1,123 @@
+#!/usr/bin/env python3
+
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Twist
+from cv_bridge import CvBridge, CvBridgeError
+import random
+import math
+
+# Define the target stopping distance from the ball in pixels
+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."""
+    area = cv2.contourArea(contour)
+    if area <= 0:
+        return False
+    circularity = 4 * np.pi * area / (2 * np.pi * radius) ** 2
+    return 0.7 <= circularity <= 1.3
+
+def find_red_ball(image):
+    """Detect red ball in the image and return its position and radius if found."""
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+    lower_red1 = np.array([0, 120, 70])
+    upper_red1 = np.array([10, 255, 255])
+    lower_red2 = np.array([170, 120, 70])
+    upper_red2 = np.array([180, 255, 255])
+    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
+    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
+    mask = mask1 + mask2
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+    for contour in contours:
+        (x, y), radius = cv2.minEnclosingCircle(contour)
+        if radius > 5 and is_circular(contour, radius):
+            return (int(x), int(y)), radius
+    return None, None
+
+def execute_search():
+    """Execute a search pattern: rotate 360, move randomly if the ball isn't found, and repeat."""
+    global current_state
+    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+    twist_msg = Twist()
+
+    # Rotate 360 degrees
+    twist_msg.angular.z = 0.5  # Adjust as needed
+    rotate_duration = 2 * math.pi / twist_msg.angular.z  # Full circle
+    start_time = rospy.Time.now().to_sec()
+    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):
+    global current_state, ball_position, ball_radius
+    try:
+        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
+    except CvBridgeError as e:
+        rospy.logerr(e)
+
+    center, radius = find_red_ball(cv_image)
+    ball_position = center
+    ball_radius = radius
+
+    if center and radius >= RADIUS_THRESHOLD:
+        # Ball found and close enough, execute half-circle
+        current_state = "maneuvering"
+        execute_half_circle()
+    elif current_state == "searching":
+        # Ball not found or not close enough, continue searching
+        execute_search()
+
+def execute_half_circle():
+    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
+    twist_msg.linear.x = 0.05  # Slight forward movement
+    duration = 4  # Adjust for a proper half-circle based on your robot
+    start_time = rospy.Time.now().to_sec()
+    while rospy.Time.now().to_sec() - start_time < duration:
+        pub.publish(twist_msg)
+    twist_msg.angular.z = 0
+    twist_msg.linear.x = 0
+    pub.publish(twist_msg)  # Stop
+    current_state = "approaching"
+
+def shutdown():
+    rospy.loginfo("Shutting down")
+    cmd_vel_pub.publish(Twist())
+    cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    rospy.init_node('redball_chase', anonymous=False)
+    bridge = CvBridge()
+    cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
+    rospy.Subscriber("/camera/image", Image, image_callback)
+
+    rospy.on_shutdown(shutdown)
+    cv2.namedWindow("Red Ball Detection", cv2.WINDOW_NORMAL)
+
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        rospy.loginfo("Red ball follower node terminated.")
+

redball_detection.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+
+import rospy
+import cv2
+import numpy as np
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+
+def is_circular(contour, radius):
+    """Check if the contour is circular enough to be considered a ball."""
+    area = cv2.contourArea(contour)
+    if area <= 0:
+        return False
+    circularity = 4 * np.pi * area / (2 * np.pi * radius) ** 2
+    # Circular enough if circularity is close to 1; adjust threshold as needed
+    return 0.7 <= circularity <= 1.3
+
+def find_red_ball(image):
+    # Convert the image to HSV color space
+    hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
+
+    # Adjusted Red color range in HSV; might need fine-tuning
+    lower_red1 = np.array([0, 120, 70])
+    upper_red1 = np.array([10, 255, 255])
+    lower_red2 = np.array([170, 120, 70])
+    upper_red2 = np.array([180, 255, 255])
+
+    mask1 = cv2.inRange(hsv, lower_red1, upper_red1)
+    mask2 = cv2.inRange(hsv, lower_red2, upper_red2)
+
+    # Combine the masks
+    mask = mask1 + mask2
+
+    # Find contours in the mask
+    contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
+
+    for contour in contours:
+        (x, y), radius = cv2.minEnclosingCircle(contour)
+        if radius > 5 and is_circular(contour, radius):  # Checks size and circularity
+            center = (int(x), int(y))
+            # Draw the circle around the detected ball
+            cv2.circle(image, center, int(radius), (0, 255, 0), 2)
+            # Label the detected ball
+            cv2.putText(image, 'Red Ball', (center[0] - 20, center[1] - int(radius) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
+            break  # Assuming only one ball, break after finding the first circular object
+
+    return image
+
+def image_callback(msg):
+    try:
+        # Convert the ROS image message to OpenCV format
+        cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
+    except CvBridgeError as e:
+        print(e)
+
+    # Process the image with your function
+    processed_image = find_red_ball(cv_image)
+
+    # Display the result
+    cv2.imshow("Red Ball Detection", processed_image)
+    cv2.waitKey(3)
+
+if __name__ == '__main__':
+    rospy.init_node('red_ball_detection', anonymous=True)
+    bridge = CvBridge()
+    # Subscribe to the camera image topic
+    rospy.Subscriber("/camera/image", Image, image_callback)
+
+    # Setup window for display (optional, remove if running headless)
+    cv2.namedWindow("Red Ball Detection", cv2.WINDOW_NORMAL)
+
+    try:
+        # Start the ROS event loop
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down")
+    cv2.destroyAllWindows()