nathan.beaud
/
Dogbot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

This commit is contained in:
Nathan BEAUD 2024-04-09 17:16:17 +02:00
parent f19770db80 0d5a3ae9c0
commit f42cfe6c40
2 changed files with 196 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
RedBall_Chase.cpp Normal file
View File

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

97
redball_chase.py
View File

@ -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.")