diff --git a/RedBall_Chase.cpp b/RedBall_Chase.cpp new file mode 100644 index 0000000..619566d --- /dev/null +++ b/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 +#include +#include +#include +#include + +class RedBallChaser { +public: + RedBallChaser() { + cmd_vel_pub = nh.advertise("/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> 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 &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; +} + diff --git a/redball_chase.py b/redball_chase.py index 3d7390d..2e29944 100644 --- a/redball_chase.py +++ b/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 -TARGET_DISTANCE = 1.0 +# 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.""" @@ -41,21 +42,34 @@ def find_red_ball(image): return (int(x), int(y)), radius return None, None -def lidar_callback(data): - """Process LiDAR data to adjust DogBot's behavior based on distance to obstacles.""" +def execute_search(): + """Execute a search pattern: rotate 360, move randomly if the ball isn't found, and repeat.""" global current_state - if current_state == "approaching": - # Assuming data.ranges is a list of distance measurements - # You might need to process this data differently based on your LiDAR setup - distance_to_ball = min(data.ranges) - if distance_to_ball <= TARGET_DISTANCE: - current_state = "maneuvering" - # Placeholder for function to initiate maneuvering behind the ball - execute_half_circle() + 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): - """Process image data to detect the red ball and control DogBot's movement.""" - global current_state, ball_position + global current_state, ball_position, ball_radius 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: - x, _ = center - err = x - cv_image.shape[1] / 2 - if current_state == "searching" or current_state == "approaching": - current_state = "approaching" - if abs(err) > 100: # Need to rotate DogBot to align with the ball - 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) + 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(): - """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() - - # Set angular velocity for a half-circle rotation - twist_msg.angular.z = 0.5 # Positive for counter-clockwise, negative for clockwise - - # 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 + 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) - 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) - - # Update the state to indicate the robot is now positioned behind the ball and ready to push it - current_state = "pushing" - + pub.publish(twist_msg) # Stop + current_state = "approaching" 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.") +