diff --git a/catkin_ws/src/CMakeLists.txt b/catkin_ws/src/CMakeLists.txt
deleted file mode 120000
index 2016816..0000000
--- a/catkin_ws/src/CMakeLists.txt
+++ /dev/null
@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake
\ No newline at end of file
diff --git a/catkin_ws/src/ball_tracking/ball_tracking.launch b/catkin_ws/src/ball_tracking/ball_tracking.launch
index 63c13c6..af5f54c 100644
--- a/catkin_ws/src/ball_tracking/ball_tracking.launch
+++ b/catkin_ws/src/ball_tracking/ball_tracking.launch
@@ -8,9 +8,6 @@
-
-
-
diff --git a/catkin_ws/src/ball_tracking/ball_tracking_exp.launch b/catkin_ws/src/ball_tracking/ball_tracking_exp.launch
new file mode 100644
index 0000000..2b68a67
--- /dev/null
+++ b/catkin_ws/src/ball_tracking/ball_tracking_exp.launch
@@ -0,0 +1,20 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/ball_tracking/src/aruco_area.py b/catkin_ws/src/ball_tracking/src/aruco_area.py
index 834ce59..e5a646b 100755
--- a/catkin_ws/src/ball_tracking/src/aruco_area.py
+++ b/catkin_ws/src/ball_tracking/src/aruco_area.py
@@ -25,17 +25,51 @@ def find_polygon_centroid(centers):
centroid_y = np.mean([center[1] for center in centers], axis=0)
return centroid_x, centroid_y
+def sort_points_clockwise(centers):
+ """Sort the points in a clockwise order based on their angles relative to the centroid."""
+ if not centers:
+ return []
+
+ centroid = np.mean(centers, axis=0)
+ def angle_from_centroid(center):
+ return np.arctan2(center[1] - centroid[1], center[0] - centroid[0])
+ sorted_centers = sorted(centers, key=angle_from_centroid)
+ return sorted_centers
+
+def calculate_side_averages(sorted_centers):
+ """Calculate the average of the x and y coordinates for all sides."""
+ # Assuming the sorted centers are in clockwise order: top-left, top-right, bottom-right, bottom-left
+ right_average = np.mean([sorted_centers[1], sorted_centers[2]], axis=0)
+ left_average = np.mean([sorted_centers[0], sorted_centers[3]], axis=0)
+ top_average = np.mean([sorted_centers[0], sorted_centers[1]], axis=0)
+ bottom_average = np.mean([sorted_centers[2], sorted_centers[3]], axis=0)
+ return left_average, right_average, top_average, bottom_average
+
+def publish_side_points(left_average, right_average, top_average, bottom_average):
+ """Publish the average points for all sides with specified Z coordinates."""
+ left_point = Point(x=left_average[0], y=left_average[1], z=1) # Z=1 for left
+ right_point = Point(x=right_average[0], y=right_average[1], z=2) # Z=2 for right
+ top_point = Point(x=top_average[0], y=top_average[1], z=3) # Z=3 for top
+ bottom_point = Point(x=bottom_average[0], y=bottom_average[1], z=4) # Z=4 for bottom
+ side_pub.publish(left_point)
+ side_pub.publish(right_point)
+ side_pub.publish(top_point)
+ side_pub.publish(bottom_point)
+ sizePoint = Point(x=abs(bottom_average[0]-top_average[0]), y=abs(left_average[1]-right_average[1]), z=4)
+ size_terrain.publish(sizePoint)
+
def image_callback(msg):
- global pub, marker_pub
+ global pub, marker_pub, side_pub
bridge = CvBridge()
frame = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejected = aruco.detectMarkers(gray, arucoDict)
- if ids is not None and len(ids) == 4: # Ensure exactly four markers are detected for a quadrilateral
+ if ids is not None and len(ids) >= 4: # Adjust this condition based on your setup
marker_centers = calculate_marker_centers(corners)
- centerX, centerY = find_polygon_centroid(marker_centers)
+ sorted_centers = sort_points_clockwise(marker_centers)
+ centerX, centerY = find_polygon_centroid(sorted_centers)
if centerX is not None and centerY is not None:
origin = Point()
@@ -44,49 +78,53 @@ def image_callback(msg):
origin.z = 0
pub.publish(origin)
- # Publish the centers of the markers as the midpoints of the polygon sides
+ left_average, right_average, top_average, bottom_average = calculate_side_averages(sorted_centers)
+ publish_side_points(left_average - np.array([centerX, centerY]),
+ right_average - np.array([centerX, centerY]),
+ top_average - np.array([centerX, centerY]),
+ bottom_average - np.array([centerX, centerY]))
+
+ # Publish the edges of the polygon
marker = Marker()
- marker.header.frame_id = "camera_link" # Adjust if necessary
+ marker.header.frame_id = "camera_link"
marker.header.stamp = rospy.Time.now()
- marker.ns = "polygon_sides_midpoints"
+ marker.ns = "polygon_edges"
marker.id = 0
marker.type = Marker.LINE_STRIP
marker.action = Marker.ADD
- marker.scale.x = 0.02 # Line width
- marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0) # Color
+ marker.scale.x = 0.02
+ marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)
marker.lifetime = rospy.Duration()
- for center in marker_centers:
+ for center in sorted_centers:
p = Point()
p.x, p.y = center
+ p.z = 0
marker.points.append(p)
-
- # Close the polygon by adding the first point again to connect the last and first midpoints
- if marker_centers:
+ if sorted_centers:
p = Point()
- p.x, p.y = marker_centers[0]
+ p.x, p.y = sorted_centers[0]
+ p.z = 0
marker.points.append(p)
marker_pub.publish(marker)
- # Optional: Visualize the centroid and detected markers
cv2.circle(frame, (int(centerX), int(centerY)), 5, (0, 255, 0), -1)
aruco.drawDetectedMarkers(frame, corners, ids)
- # Display the frame
cv2.imshow('Frame', frame)
cv2.waitKey(1)
def main():
- global pub, marker_pub, arucoDict
+ global pub, marker_pub, side_pub, arucoDict, size_terrain
rospy.init_node('aruco_origin_publisher', anonymous=True)
pub = rospy.Publisher('/aruco_origin', Point, queue_size=10)
- marker_pub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
-
- # Initialize the ArUco dictionary
+ marker_pub = rospy.Publisher('/polygon_edges', Marker, queue_size=10)
+ side_pub = rospy.Publisher('/LR_edge_centers', Point, queue_size=10)
+ size_terrain = rospy.Publisher('/width_length', Point, queue_size=10)
+
arucoDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
- # Subscribe to the camera_image topic
rospy.Subscriber('camera_image', Image, image_callback)
rospy.spin()
diff --git a/catkin_ws/src/ball_tracking/src/ball_prediction.py b/catkin_ws/src/ball_tracking/src/ball_prediction.py
new file mode 100755
index 0000000..97e07e1
--- /dev/null
+++ b/catkin_ws/src/ball_tracking/src/ball_prediction.py
@@ -0,0 +1,58 @@
+#!/usr/bin/env python3
+
+import rospy
+from geometry_msgs.msg import Point
+from std_msgs.msg import Header
+import numpy as np
+
+class BallMovementPredictor:
+ def __init__(self):
+ rospy.init_node('ball_movement_predictor', anonymous=True)
+
+ self.ball_positions = []
+ self.forecast_length = 5 # Predict the ball's position 5 steps into the future
+
+ # Subscriber to the ball's current position
+ self.ball_sub = rospy.Subscriber('/ball_coordinates', Point, self.ball_callback)
+
+ # Publisher for the ball's forecasted future position
+ self.forecast_pub = rospy.Publisher('/ball_forecast', Point, queue_size=10)
+
+ def ball_callback(self, msg):
+ # Update the list of recent ball positions
+ self.ball_positions.append((msg.x, msg.y))
+
+ # Keep only the last N positions for prediction
+ if len(self.ball_positions) > 5:
+ self.ball_positions.pop(0)
+
+ # Predict the ball's future position and publish it
+ forecasted_position = self.predict_future_position()
+ if forecasted_position is not None:
+ forecast_msg = Point(x=forecasted_position[0], y=forecasted_position[1], z=0)
+ self.forecast_pub.publish(forecast_msg)
+
+ def predict_future_position(self):
+ if len(self.ball_positions) < 2:
+ return None # Not enough data to predict
+
+ # Simple linear prediction based on the last two positions
+ x_positions, y_positions = zip(*self.ball_positions)
+ x_velocity = x_positions[-1] - x_positions[-2]
+ y_velocity = y_positions[-1] - y_positions[-2]
+
+ forecasted_x = x_positions[-1] + x_velocity * self.forecast_length
+ forecasted_y = y_positions[-1] + y_velocity * self.forecast_length
+
+ return (forecasted_x, forecasted_y)
+
+ def run(self):
+ rospy.spin()
+
+if __name__ == '__main__':
+ try:
+ predictor = BallMovementPredictor()
+ predictor.run()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/catkin_ws/src/ball_tracking/src/dummy_sub.py b/catkin_ws/src/ball_tracking/src/dummy_sub.py
new file mode 100755
index 0000000..6b6bcf2
--- /dev/null
+++ b/catkin_ws/src/ball_tracking/src/dummy_sub.py
@@ -0,0 +1,25 @@
+#!/usr/bin/env python3
+import rospy
+from geometry_msgs.msg import Point
+
+def callback(data):
+ """Callback function that is called when a message is received on the /trigger topic."""
+ rospy.loginfo(f"Received point: (x: {data.x}, y: {data.y}, z: {data.z})")
+
+def listener():
+ """Sets up the subscriber node."""
+ # Initialize the node with the name 'point_subscriber'
+ rospy.init_node('PONG_PADDLES', anonymous=False)
+
+ # Subscribe to the '/trigger' topic with the callback function
+ rospy.Subscriber("/trigger", Point, callback)
+
+ # Keep the program alive until it is stopped with Ctrl+C
+ rospy.spin()
+
+if __name__ == '__main__':
+ try:
+ listener()
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/catkin_ws/src/ball_tracking/src/paddle_action.py b/catkin_ws/src/ball_tracking/src/paddle_action.py
index a34d0d4..7241896 100755
--- a/catkin_ws/src/ball_tracking/src/paddle_action.py
+++ b/catkin_ws/src/ball_tracking/src/paddle_action.py
@@ -2,7 +2,6 @@
import rospy
from geometry_msgs.msg import Point
-from std_msgs.msg import Int32 # This import is no longer needed, but it can be kept if used elsewhere
class BallEdgeTrigger:
def __init__(self):
@@ -12,8 +11,10 @@ class BallEdgeTrigger:
self.ball_sub = rospy.Subscriber('/ball_coordinates', Point, self.ball_callback)
self.edge_sub = rospy.Subscriber('/LR_edge_centers', Point, self.edge_callback)
- # Publisher
- self.trigger_pub = rospy.Publisher('/trigger', Point, queue_size=10) # Change to Point
+ # Publishers
+ self.trigger_pub = rospy.Publisher('/trigger', Point, queue_size=10)
+ # Additional publisher for /trigger_mm with modified coordinates
+ self.trigger_mm_pub = rospy.Publisher('/trigger_mm', Point, queue_size=10)
# To store edge centers
self.edge_centers = [] # List to hold edge center points
@@ -25,14 +26,21 @@ class BallEdgeTrigger:
def ball_callback(self, msg):
# Initialize a Point message with default values (Z=0 means no trigger)
trigger_point = Point(x=msg.x, y=msg.y, z=0)
+ # Prepare the modified Point message for /trigger_mm
+ trigger_mm_point = Point(x=msg.x * 0.04, y=msg.y * 0.028, z=0)
+
for edge in self.edge_centers:
if abs(msg.x - edge.x) <= 20:
# Set Z to 1 if the ball is approaching the left edge (Z=1), or to 2 if approaching the right (Z=2)
trigger_point.z = edge.z
+ # Apply the same Z logic to the modified point for /trigger_mm
+ trigger_mm_point.z = edge.z
break # Stop checking after the first trigger condition is met
- # Publish the trigger Point message
+ # Publish the original trigger Point message
self.trigger_pub.publish(trigger_point)
+ # Publish the modified trigger Point message on /trigger_mm
+ self.trigger_mm_pub.publish(trigger_mm_point)
def run(self):
rospy.spin()
diff --git a/catkin_ws/src/ball_tracking/src/prediction_monitor.py b/catkin_ws/src/ball_tracking/src/prediction_monitor.py
new file mode 100755
index 0000000..e92dcea
--- /dev/null
+++ b/catkin_ws/src/ball_tracking/src/prediction_monitor.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+
+import rospy
+import cv2
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point
+
+class BallPredictionVisualizer:
+ def __init__(self):
+ rospy.init_node('ball_prediction_visualizer', anonymous=True)
+
+ self.bridge = CvBridge()
+ self.forecasted_position_aruco = None
+ self.aruco_origin = None
+
+ self.image_sub = rospy.Subscriber('/camera_image', Image, self.image_callback)
+ self.forecast_sub = rospy.Subscriber('/ball_forecast', Point, self.forecast_callback)
+ self.origin_sub = rospy.Subscriber('/aruco_origin', Point, self.origin_callback)
+
+ def forecast_callback(self, msg):
+ self.forecasted_position_aruco = (int(msg.x), int(msg.y))
+
+ def origin_callback(self, msg):
+ self.aruco_origin = (int(msg.x), int(msg.y))
+
+ def image_callback(self, msg):
+ cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
+
+ if self.aruco_origin:
+ # Draw the ArUco origin for verification
+ cv2.circle(cv_image, self.aruco_origin, 10, (0, 255, 0), -1) # Green dot for the origin
+
+ if self.forecasted_position_aruco and self.aruco_origin:
+ # Adjust for the origin and possibly invert the y-axis
+ adjusted_position = (
+ self.forecasted_position_aruco[0] + self.aruco_origin[0],
+ self.forecasted_position_aruco[1] + self.aruco_origin[1]
+ )
+ cv2.circle(cv_image, adjusted_position, 10, (0, 0, 255), -1) # Red dot for the predicted position
+
+ cv2.imshow("Ball Prediction", cv_image)
+ cv2.waitKey(1)
+
+ def run(self):
+ rospy.spin()
+
+if __name__ == '__main__':
+ try:
+ visualizer = BallPredictionVisualizer()
+ visualizer.run()
+ except rospy.ROSInterruptException:
+ cv2.destroyAllWindows()
+
diff --git a/catkin_ws/src/ball_tracking/src/tracker_aruco.py b/catkin_ws/src/ball_tracking/src/tracker_aruco.py
index 0eda697..f8714e4 100755
--- a/catkin_ws/src/ball_tracking/src/tracker_aruco.py
+++ b/catkin_ws/src/ball_tracking/src/tracker_aruco.py
@@ -50,9 +50,12 @@ def image_callback(msg):
adjusted_x = x - origin[0]
adjusted_y = y - origin[1]
ball_point = Point(x=adjusted_x, y=adjusted_y, z=0)
+ ball_point_mm = Point(x=adjusted_x*0.04, y=adjusted_y*0.028, z=0)
else:
ball_point = Point(x=x, y=y, z=0)
+ ball_point_mm = Point(x=x*0.04, y=y*0.028, z=0)
coord_pub.publish(ball_point)
+ coord_pub_mm.publish(ball_point_mm)
# Display the frames for debugging
cv2.imshow('frame', frame)
@@ -71,6 +74,7 @@ rospy.Subscriber('camera_image', Image, image_callback)
# ROS Publisher for the ball coordinates
coord_pub = rospy.Publisher('/ball_coordinates', Point, queue_size=10)
+coord_pub_mm = rospy.Publisher('/ball_coordinates_mm', Point, queue_size=10)
# Trackbars setup for HSV thresholding
cv2.namedWindow('settings')
diff --git a/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py b/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py
deleted file mode 100755
index cf6ca3b..0000000
--- a/catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py
+++ /dev/null
@@ -1,125 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-from geometry_msgs.msg import Point
-from sensor_msgs.msg import Image
-from visualization_msgs.msg import Marker
-from std_msgs.msg import ColorRGBA
-import cv2
-import cv2.aruco as aruco
-import numpy as np
-from cv_bridge import CvBridge
-
-def calculate_marker_centers(corners):
- """Calculate the center of each ArUco marker."""
- centers = []
- for corner in corners:
- cx = np.mean([point[0] for point in corner[0]], axis=0)
- cy = np.mean([point[1] for point in corner[0]], axis=0)
- centers.append((cx, cy))
- return centers
-
-def find_polygon_centroid(centers):
- """Calculate the centroid of the polygon formed by the marker centers."""
- centroid_x = np.mean([center[0] for center in centers], axis=0)
- centroid_y = np.mean([center[1] for center in centers], axis=0)
- return centroid_x, centroid_y
-
-def sort_points_clockwise(centers):
- """Sort the points in a clockwise order based on their angles relative to the centroid."""
- if not centers:
- return []
-
- centroid = np.mean(centers, axis=0)
- def angle_from_centroid(center):
- return np.arctan2(center[1] - centroid[1], center[0] - centroid[0])
- sorted_centers = sorted(centers, key=angle_from_centroid)
- return sorted_centers
-
-def calculate_side_averages(sorted_centers):
- """Calculate the average of the x and y coordinates for the left and right sides."""
- # Assuming the sorted centers are in clockwise order: top-left, top-right, bottom-right, bottom-left
- right_average = np.mean([sorted_centers[1], sorted_centers[2]], axis=0)
- left_average = np.mean([sorted_centers[0], sorted_centers[3]], axis=0)
- return left_average, right_average
-
-def publish_side_points(left_average, right_average):
- """Publish the average points for the left and right sides with specified Z coordinates."""
- left_point = Point(x=left_average[0], y=left_average[1], z=1) # Z=1 for left
- right_point = Point(x=right_average[0], y=right_average[1], z=2) # Z=2 for right
- side_pub.publish(left_point)
- side_pub.publish(right_point)
-
-def image_callback(msg):
- global pub, marker_pub, side_pub
- bridge = CvBridge()
- frame = bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
-
- gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- corners, ids, rejected = aruco.detectMarkers(gray, arucoDict)
-
- if ids is not None and len(ids) >= 4: # Adjust this condition based on your setup
- marker_centers = calculate_marker_centers(corners)
- sorted_centers = sort_points_clockwise(marker_centers)
- centerX, centerY = find_polygon_centroid(sorted_centers)
-
- if centerX is not None and centerY is not None:
- origin = Point()
- origin.x = centerX
- origin.y = centerY
- origin.z = 0
- pub.publish(origin)
-
- left_average, right_average = calculate_side_averages(sorted_centers)
- publish_side_points(left_average - np.array([centerX, centerY]),
- right_average - np.array([centerX, centerY]))
-
- # Publish the edges of the polygon
- marker = Marker()
- marker.header.frame_id = "camera_link"
- marker.header.stamp = rospy.Time.now()
- marker.ns = "polygon_edges"
- marker.id = 0
- marker.type = Marker.LINE_STRIP
- marker.action = Marker.ADD
- marker.scale.x = 0.02
- marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)
- marker.lifetime = rospy.Duration()
-
- for center in sorted_centers:
- p = Point()
- p.x, p.y = center
- p.z = 0
- marker.points.append(p)
- if sorted_centers:
- p = Point()
- p.x, p.y = sorted_centers[0]
- p.z = 0
- marker.points.append(p)
-
- marker_pub.publish(marker)
-
- cv2.circle(frame, (int(centerX), int(centerY)), 5, (0, 255, 0), -1)
- aruco.drawDetectedMarkers(frame, corners, ids)
-
- cv2.imshow('Frame', frame)
- cv2.waitKey(1)
-
-def main():
- global pub, marker_pub, side_pub, arucoDict
- rospy.init_node('aruco_origin_publisher', anonymous=True)
- pub = rospy.Publisher('/aruco_origin', Point, queue_size=10)
- marker_pub = rospy.Publisher('/polygon_edges', Marker, queue_size=10)
- side_pub = rospy.Publisher('/LR_edge_centers', Point, queue_size=10)
-
- arucoDict = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
-
- rospy.Subscriber('camera_image', Image, image_callback)
-
- rospy.spin()
-
- cv2.destroyAllWindows()
-
-if __name__ == '__main__':
- main()
-
diff --git a/catkin_ws/src/ball_tracking/src/trackernew.py b/catkin_ws/src/ball_tracking/src/trackernew.py
deleted file mode 100755
index b8fb5b4..0000000
--- a/catkin_ws/src/ball_tracking/src/trackernew.py
+++ /dev/null
@@ -1,95 +0,0 @@
-#!/usr/bin/env python3
-
-import rospy
-from geometry_msgs.msg import Point
-import cv2
-import numpy as np
-
-def nothing(x):
- pass
-
-# Initialize the ROS node
-rospy.init_node('ball_tracking_node', anonymous=True)
-
-# Create a ROS publisher for the ball's coordinates
-coord_pub = rospy.Publisher('/ball_coordinates', Point, queue_size=10)
-
-# Create a window for the trackbars
-cv2.namedWindow('settings')
-
-# Create trackbars for adjusting the HSV range
-cv2.createTrackbar('Lower-H', 'settings', 0, 179, nothing)
-cv2.createTrackbar('Lower-S', 'settings', 100, 255, nothing)
-cv2.createTrackbar('Lower-V', 'settings', 100, 255, nothing)
-cv2.createTrackbar('Upper-H', 'settings', 22, 179, nothing)
-cv2.createTrackbar('Upper-S', 'settings', 255, 255, nothing)
-cv2.createTrackbar('Upper-V', 'settings', 255, 255, nothing)
-
-# Attempt to open the video capture
-cap = cv2.VideoCapture(0)
-
-# Check if the camera opened successfully
-if not cap.isOpened():
- print("Error: Could not open camera.")
- exit()
-
-while not rospy.is_shutdown():
- # Capture frame-by-frame
- ret, frame = cap.read()
- if not ret:
- print("Can't receive frame (stream end?). Exiting ...")
- break
-
- # Convert the captured frame to HSV
- hsv = cv2.cvtColor(frame, cv2.COLOR_BGR2HSV)
-
- # Get the current positions of the trackbars
- lh = cv2.getTrackbarPos('Lower-H', 'settings')
- ls = cv2.getTrackbarPos('Lower-S', 'settings')
- lv = cv2.getTrackbarPos('Lower-V', 'settings')
- uh = cv2.getTrackbarPos('Upper-H', 'settings')
- us = cv2.getTrackbarPos('Upper-S', 'settings')
- uv = cv2.getTrackbarPos('Upper-V', 'settings')
-
- # Define the HSV range for the orange color
- lower_orange = np.array([lh, ls, lv])
- upper_orange = np.array([uh, us, uv])
-
- # Threshold the HSV image to only get the orange colors
- mask = cv2.inRange(hsv, lower_orange, upper_orange)
- res = cv2.bitwise_and(frame, frame, mask=mask)
-
- # Find contours in the mask
- contours, _ = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
- center = None
-
- if contours:
- # Find the largest contour in the mask
- c = max(contours, key=cv2.contourArea)
- ((x, y), radius) = cv2.minEnclosingCircle(c)
-
- if radius > 10: # Minimum radius threshold
- # Draw the circle and centroid on the frame
- cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
- center = (int(x), int(y))
- cv2.circle(frame, center, 5, (0, 0, 255), -1)
-
- # Publish the ball's coordinates
- point_msg = Point()
- point_msg.x = x
- point_msg.y = y
- point_msg.z = 0 # Z-coordinate is not applicable here
- coord_pub.publish(point_msg)
-
- # Display the original and the result
- cv2.imshow('frame', frame)
- cv2.imshow('mask', mask)
- cv2.imshow('res', res)
-
- if cv2.waitKey(1) & 0xFF == 27: # ESC key to break
- break
-
-# When everything done, release the capture and destroy all windows
-cap.release()
-cv2.destroyAllWindows()
-#Minabebis :3
diff --git a/catkin_ws/src/joystick_ros/CMakeLists.txt b/catkin_ws/src/joystick_ros/CMakeLists.txt
index 51dc825..da66e85 100644
--- a/catkin_ws/src/joystick_ros/CMakeLists.txt
+++ b/catkin_ws/src/joystick_ros/CMakeLists.txt
@@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
+ serial
)
## System dependencies are found with CMake's conventions
@@ -105,7 +106,7 @@ find_package(catkin REQUIRED COMPONENTS
catkin_package(
# INCLUDE_DIRS include
# LIBRARIES joystick_ros
-# CATKIN_DEPENDS roscpp rospy std_msgs
+ CATKIN_DEPENDS roscpp rospy std_msgs serial
# DEPENDS system_lib
)
@@ -133,7 +134,9 @@ include_directories(
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/joystick_ros_node.cpp)
+add_executable(readSerial src/readSerial.cpp)
+add_executable(parser src/parser.cpp)
+add_executable(position_ctrl src/position_ctrl.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -144,7 +147,9 @@ include_directories(
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
+target_link_libraries(readSerial ${catkin_LIBRARIES})
+target_link_libraries(parser ${catkin_LIBRARIES})
+target_link_libraries(position_ctrl ${catkin_LIBRARIES})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
diff --git a/catkin_ws/src/joystick_ros/joystick_ros.launch b/catkin_ws/src/joystick_ros/joystick_ros.launch
new file mode 100644
index 0000000..f50fa1f
--- /dev/null
+++ b/catkin_ws/src/joystick_ros/joystick_ros.launch
@@ -0,0 +1,15 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/joystick_ros/package.xml b/catkin_ws/src/joystick_ros/package.xml
index 834b245..6f78a1b 100644
--- a/catkin_ws/src/joystick_ros/package.xml
+++ b/catkin_ws/src/joystick_ros/package.xml
@@ -52,12 +52,15 @@
roscpp
rospy
std_msgs
+ serial
roscpp
rospy
std_msgs
+ serial
roscpp
rospy
std_msgs
+ serial
diff --git a/catkin_ws/src/joystick_ros/src/parser.cpp b/catkin_ws/src/joystick_ros/src/parser.cpp
index a072ffc..ffcdeca 100644
--- a/catkin_ws/src/joystick_ros/src/parser.cpp
+++ b/catkin_ws/src/joystick_ros/src/parser.cpp
@@ -7,6 +7,9 @@
float axis0_value = 0.0;
float axis1_value = 0.0;
+
+ros::Publisher axis_values_pub;
+
void serial_data_callback(const std_msgs::String::ConstPtr& msg) {
// Parse received string message
std::string data_str = msg->data;
@@ -31,16 +34,15 @@ int main(int argc, char** argv) {
// Initialize ROS node
ros::init(argc, argv, "serial_data_parser");
ros::NodeHandle nh;
-
+ // Initialize publisher for Axis values
+ axis_values_pub = nh.advertise("axis_values", 10);
+
// Subscribe to serial data topic
ros::Subscriber sub = nh.subscribe("serial_data", 10, serial_data_callback);
-
- // Initialize publisher for Axis values
- ros::Publisher axis_values_pub = nh.advertise("axis_values", 10);
-
// Spin ROS node
ros::spin();
return 0;
}
+
diff --git a/catkin_ws/src/joystick_ros/src/readSerial.cpp b/catkin_ws/src/joystick_ros/src/readSerial.cpp
index 3a42923..8133a24 100644
--- a/catkin_ws/src/joystick_ros/src/readSerial.cpp
+++ b/catkin_ws/src/joystick_ros/src/readSerial.cpp
@@ -28,7 +28,6 @@ void readSerial(const std::string& port, uint32_t baud_rate) {
serial_pub.publish(msg);
}
ros::spinOnce();
- ros::Duration(0.1).sleep(); // Sleep for 0.1 second
}
ser.close();
@@ -37,13 +36,9 @@ void readSerial(const std::string& port, uint32_t baud_rate) {
int main(int argc, char** argv) {
ros::init(argc, argv, "serial_reader");
- if (argc != 3) {
- ROS_ERROR("Usage: rosrun your_package_name serial_reader ");
- return 1;
- }
- std::string port = "COM14";
- uint32_t baud_rate = 9600;
+ std::string port = "/dev/ttyACM0"; // Get serial port from command-line argument
+ uint32_t baud_rate = 9600; // Convert string to integer
readSerial(port, baud_rate);
diff --git a/catkin_ws/src/poppy_ros/CMakeLists.txt b/catkin_ws/src/poppy_ros/CMakeLists.txt
new file mode 100644
index 0000000..b97c741
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/CMakeLists.txt
@@ -0,0 +1,241 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(poppy_ros)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+set (OpenCV_DIR "/usr/lib/opencv")
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ roscpp
+ rospy
+ std_msgs
+# message_generation
+)
+find_package(OpenCV REQUIRED COMPONENTS
+ core
+)
+
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+#add_service_files(
+# FILES
+# ik.srv
+#)
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs
+# )
+#generate_messages(
+# DEPENDENCIES
+# std_msgs
+#)
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include
+ LIBRARIES poppy_ros
+ CATKIN_DEPENDS roscpp rospy std_msgs
+ #DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ /home/alexandre/Software/toolkit-dynamixel/include
+ /home/alexandre/Software/toolkit-kinematics/include
+ /home/alexandre/Software/toolkit-dynamixel
+ /home/alexandre/Software/toolkit-kinematics
+ ${OpenCV_INCLUDE_DIRS}
+ /home/alexandre/Software/eigen/Eigen/include
+ /home/alexandre/Software/eigen/Eigen
+ ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/poppy_ros.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/poppy_ros_node.cpp)
+add_executable(poppy_ros src/poppy_ros.cpp /home/alexandre/Software/toolkit-dynamixel/src/DynamixelHandler.cpp)
+add_executable(poppy_test src/poppy_test.cpp)
+add_executable(poppy_pong_iv src/poppy_pong_iv.cpp)
+add_executable(poppy_track_x src/poppy_track_x.cpp)
+
+#add_executable(ik_server src/ik_server.cpp /home/alexandre/Software/toolkit-kinematics/src/Kinematics.cpp /home/alexandre/Software/toolkit-kinematics/src/RotationMatrix.cpp /home/alexandre/Software/toolkit-kinematics/src/TranslationMatrix.cpp /home/alexandre/Software/toolkit-kinematics/src/TransformationMatrix.cpp)
+
+#add_executable(ik_client src/ik_client.cpp)
+
+add_executable(sim_poppy_joint src/sim_poppy_joint.cpp)
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# 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}
+# )
+target_link_libraries(poppy_ros dxl_x64_cpp rt ${catkin_LIBRARIES})
+target_link_libraries(poppy_test ${catkin_LIBRARIES})
+target_link_libraries(poppy_pong_iv ${catkin_LIBRARIES})
+target_link_libraries(poppy_track_x ${catkin_LIBRARIES})
+
+#target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
+
+#target_link_libraries(ik_client ${catkin_LIBRARIES})
+target_link_libraries(sim_poppy_joint ${catkin_LIBRARIES})
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_poppy_ros.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/catkin_ws/src/poppy_ros/package.xml b/catkin_ws/src/poppy_ros/package.xml
new file mode 100644
index 0000000..62de016
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/package.xml
@@ -0,0 +1,68 @@
+
+
+ poppy_ros
+ 0.0.0
+ The poppy_ros package
+
+
+
+
+ lucas
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+ roscpp
+ rospy
+ std_msgs
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/poppy_ros/poppy_ros.launch b/catkin_ws/src/poppy_ros/poppy_ros.launch
new file mode 100644
index 0000000..86a480a
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/poppy_ros.launch
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp b/catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp
new file mode 100644
index 0000000..4b4baa2
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp
@@ -0,0 +1,105 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/Point.h"
+#include
+#include
+#include
+#include // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
+#include // Inclure la bibliothèque cmath pour atan2
+
+// Global variables
+float _fps = 10.0f; // Hz
+int _nbJoints = 6;
+float _minJointCmd = 0;
+float _maxJointCmd = 1023;
+float _minJointAngle = -180.0f;
+float _maxJointAngle = 180.0f;
+float L1 = 65;
+float L2 = 55;
+float angleBase = 100;
+bool hitting_need = false;
+ros::Publisher _jointPositionPublisher;
+
+float get_q2(float x, float y)
+{
+ float q2; // Déclarer un tableau statique de 2 éléments de type float
+ q2 = acos((x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2));
+
+ return q2;
+}
+
+float get_q1(float x, float y)
+{
+ float q1;
+ float q2 = get_q2(x, y);
+
+ q1 = atan2(y, x) - atan2(L2 * sin(q2), L1 + L2 * cos(q2));
+
+ return q1;
+}
+
+void posCMDCallback(const geometry_msgs::Point& joint_pos)
+{
+ float pos_x = joint_pos.x;
+ float pos_y = joint_pos.y;
+ if (hitting_need == true)
+ {
+ pos_x += 30;
+ }
+ if (sqrt(pow(pos_x, 2) + pow(joint_pos.y, 2)) <= 120)
+ {
+ geometry_msgs::Twist joint_cmd;
+ float q1 = get_q1(pos_x, joint_pos.y);
+ float q2 = get_q2(pos_x, joint_pos.y);
+ // stores them into a msg
+ joint_cmd.linear.x = q1*(180.0f/3.141592f);
+ joint_cmd.linear.y = angleBase;
+ joint_cmd.linear.z = q2*(180.0f/3.141592f);
+ joint_cmd.angular.x = (-q1-q2)*(180.0f/3.141592f); // end-effector orientation
+
+ // publish the Twist message to the joint_position topic
+ _jointPositionPublisher.publish(joint_cmd);
+ }
+}
+
+void hittingCommand(const geometry_msgs::Point& trigger_signal) {
+ if (trigger_signal.z == 2){
+ hitting_need = true;
+ } else {
+ hitting_need = false;
+ }
+}
+
+int main(int argc, char** argv)
+{
+ // create a node called poppy_ik
+ ros::init(argc, argv, "poppy_ik");
+
+ // create a node handle
+ ros::NodeHandle nh;
+
+ // create a publisher to joint_cmd topic
+ _jointPositionPublisher = nh.advertise("joint_cmd", 1);
+ // create a subscriber to position_cmd
+ ros::Subscriber sub = nh.subscribe("position_cmd", 1, posCMDCallback);
+ ros::Subscriber sub_trigger = nh.subscribe("trigger", 1, hittingCommand);
+
+ // create a loop rate
+ ros::Rate loopRate(_fps);
+
+
+ ROS_INFO("===Launching Poppy node===");
+
+ // loop until Ctrl+C is pressed or ROS connectivity issues
+ while(ros::ok())
+ {
+ // spin once to let the process handle callback ad key stroke
+ ros::spinOnce();
+
+ // sleep the right amout of time to comply with _fps
+ loopRate.sleep();
+ }
+
+ return 0;
+}
+
diff --git a/catkin_ws/src/poppy_ros/src/poppy_ros.cpp b/catkin_ws/src/poppy_ros/src/poppy_ros.cpp
new file mode 100644
index 0000000..848057d
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/src/poppy_ros.cpp
@@ -0,0 +1,107 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "DynamixelHandler.h"
+
+// Global variables
+float _fps = 10.0f; // Hz
+
+DynamixelHandler _oDxlHandler;
+ros::Publisher _jointPositionPublisher;
+std::string _poppyDxlPortName = "/dev/ttyUSB0";
+float _poppyDxlProtocol = 2.0;
+int _poppyDxlBaudRate = 1000000;
+int _nbJoints = 6;
+float _minJointCmd = 0;
+float _maxJointCmd = 1023;
+float _minJointAngle = -180.0f;
+float _maxJointAngle = 180.0f;
+// create vector unit 16_t
+std::vector jointVec;
+
+int convertAnglesToJointCmd(float fJointAngle)
+{
+ // y = ax + b
+ float a = (_maxJointCmd-_minJointCmd) / (_maxJointAngle - _minJointAngle);
+ float b = _minJointCmd - a * _minJointAngle;
+ float jointCmd = a * fJointAngle + b;
+ return (int)jointCmd;
+}
+
+void jointCMDCallback(const geometry_msgs::Twist& joint_cmd)
+{
+ jointVec.clear();
+ //fill it w joint_cmd_values
+ jointVec.push_back(convertAnglesToJointCmd(joint_cmd.linear.x));
+ jointVec.push_back(convertAnglesToJointCmd(joint_cmd.linear.y));
+ jointVec.push_back(convertAnglesToJointCmd(joint_cmd.linear.z));
+ jointVec.push_back(convertAnglesToJointCmd(joint_cmd.angular.x));
+ jointVec.push_back(0.0);
+ jointVec.push_back(0.0);
+ //call sendTargetJOintPosition(vector) of DxlHandler
+ _oDxlHandler.sendTargetJointPosition(jointVec);
+}
+
+int main(int argc, char** argv)
+{
+ // create a node called poppy_ros
+ ros::init(argc, argv, "poppy_ros", ros::init_options::NoSigintHandler);
+ //ros::init(argc, argv, "autopilot");
+
+ // create a node handle
+ ros::NodeHandle nh;
+
+ // create a publisher to joint_position topic
+ _jointPositionPublisher = nh.advertise("joint_position", 1);
+
+ //create a subscriber
+ ros::Subscriber sub = nh.subscribe("joint_cmd",1, jointCMDCallback);
+
+ // create a loop rate
+ ros::Rate loopRate(_fps);
+
+ // create a Twist message
+ geometry_msgs::Twist jointPositionMsg;
+
+ std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+ _oDxlHandler.setDeviceName(_poppyDxlPortName);
+ _oDxlHandler.setProtocolVersion(_poppyDxlProtocol);
+ _oDxlHandler.openPort();
+ _oDxlHandler.setBaudRate(_poppyDxlBaudRate);
+ _oDxlHandler.enableTorque(false);
+ std::cout << std::endl;
+
+
+
+ ROS_INFO("===Launching Poppy node===");
+
+ // loop until Ctrl+C is pressed or ROS connectivity issues
+ while(ros::ok())
+ {
+ //===RETRIEVE Dynamixel Motor positions====
+ std::vector l_vCurrentJointPosition;
+ bool bIsReadSuccessfull = _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+
+ // stores them into a msg
+ if (bIsReadSuccessfull)
+ {
+ jointPositionMsg.linear.x = l_vCurrentJointPosition[0];
+ jointPositionMsg.linear.y = l_vCurrentJointPosition[1];
+ jointPositionMsg.linear.z = l_vCurrentJointPosition[2];
+ jointPositionMsg.angular.x = l_vCurrentJointPosition[3];
+ }
+
+ // publish the Twist message to the joint_position topic
+ _jointPositionPublisher.publish(jointPositionMsg);
+
+ // spin once to let the process handle callback ad key stroke
+ ros::spinOnce();
+
+ // sleep the right amout of time to comply with _fps
+ loopRate.sleep();
+ }
+
+ _oDxlHandler.enableTorque(false);
+ _oDxlHandler.closePort();
+
+ return 0;
+}
diff --git a/catkin_ws/src/poppy_ros/src/poppy_test.cpp b/catkin_ws/src/poppy_ros/src/poppy_test.cpp
new file mode 100644
index 0000000..8579170
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/src/poppy_test.cpp
@@ -0,0 +1,60 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+
+
+ros::Publisher _jointPositionPublisher;
+
+int main(int argc, char** argv)
+{
+ // create a node called poppy_ros
+ ros::init(argc, argv, "poppy_ros_test");
+ //ros::init(argc, argv, "autopilot");
+
+ // create a node handle
+ ros::NodeHandle nh;
+
+ // create a publisher to joint_position topic
+ _jointPositionPublisher = nh.advertise("joint_cmd", 1);
+
+
+ // create a loop rate
+ ros::Rate loopRate(1);
+
+ // create a Twist message
+ geometry_msgs::Twist jointPositionMsg;
+
+
+ int x=0;
+
+ ROS_INFO("===Launching Poppy node===");
+
+ // loop until Ctrl+C is pressed or ROS connectivity issues
+ while(ros::ok())
+ {
+
+ // stores them into a msg
+ if (x != 180)
+ {
+ jointPositionMsg.linear.x = x;
+ jointPositionMsg.linear.y = 100;
+ jointPositionMsg.linear.z = x;
+ jointPositionMsg.angular.x = x;
+ x+=10;
+ }
+ else if(x==180)
+ {
+ x=0;
+ }
+
+ // publish the Twist message to the joint_position topic
+ _jointPositionPublisher.publish(jointPositionMsg);
+
+ // spin once to let the process handle callback ad key stroke
+ ros::spinOnce();
+
+ // sleep the right amout of time to comply with _fps
+ loopRate.sleep();
+ }
+
+ return 0;
+}
diff --git a/catkin_ws/src/poppy_ros/src/poppy_track_x.cpp b/catkin_ws/src/poppy_ros/src/poppy_track_x.cpp
new file mode 100644
index 0000000..8615cc3
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/src/poppy_track_x.cpp
@@ -0,0 +1,72 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/Point.h"
+#include
+#include
+#include
+#include // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
+#include // Inclure la bibliothèque cmath pour atan2
+
+
+// Global variables
+float _fps = 10.0f; // Hz
+ros::Publisher _jointPositionPublisher;
+float std_pos = 50.0f;
+float halfwidth = 0;
+float length = 0;
+
+
+
+
+void posCMDCallback(const geometry_msgs::Point& ballPos)
+{
+ geometry_msgs::Point joint_pos;
+ // stores them into a msg
+ joint_pos.x = std_pos;
+ joint_pos.y = ballPos.y*1.3;
+ //ROS_INFO(ballPos.x + " and " + halfwidth);
+ // publish the Twist message to the joint_position topic
+ _jointPositionPublisher.publish(joint_pos);
+}
+
+void width_callback(const geometry_msgs::Point& terrainSize)
+{
+ halfwidth = terrainSize.x/2.f;
+ length = terrainSize.y;
+}
+
+int main(int argc, char** argv)
+{
+ //ros::init(argc, argv, "autopilot");
+ // create a node called poppy_ros
+ ros::init(argc, argv, "poppy_track_ball");
+
+ // create a node handle
+ ros::NodeHandle nh;
+
+ // create a publisher to joint_position topic
+ _jointPositionPublisher = nh.advertise("position_cmd", 1);
+ //create a subscriber width_length
+ ros::Subscriber sub = nh.subscribe("ball_coordinates",1, posCMDCallback);
+
+ ros::Subscriber sub2 = nh.subscribe("width_length",1, width_callback);
+
+ // create a loop rate
+ ros::Rate loopRate(_fps);
+
+ ROS_INFO("===Launching Poppy node===");
+
+ // loop until Ctrl+C is pressed or ROS connectivity issues
+ while(ros::ok())
+ {
+
+ // spin once to let the process handle callback ad key stroke
+ ros::spinOnce();
+
+ // sleep the right amout of time to comply with _fps
+ loopRate.sleep();
+ }
+
+
+ return 0;
+}
diff --git a/catkin_ws/src/poppy_ros/src/sim_poppy_joint.cpp b/catkin_ws/src/poppy_ros/src/sim_poppy_joint.cpp
new file mode 100644
index 0000000..f1a53b0
--- /dev/null
+++ b/catkin_ws/src/poppy_ros/src/sim_poppy_joint.cpp
@@ -0,0 +1,53 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+#include "sensor_msgs/JointState.h"
+
+std::vector jointVec;
+void jointCMDCallback(const geometry_msgs::Twist& joint_cmd)
+{
+ jointVec.clear();
+ //fill it w joint_cmd_values
+ jointVec.push_back(joint_cmd.linear.x);
+ jointVec.push_back(joint_cmd.linear.y);
+ jointVec.push_back(joint_cmd.linear.z);
+ jointVec.push_back(joint_cmd.angular.x);
+ jointVec.push_back(joint_cmd.angular.y);
+ jointVec.push_back(joint_cmd.angular.z);
+}
+double deg2rad(double angle)
+{
+ return -angle / 180.0 * M_PI;
+}
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "poppy_ros", ros::init_options::NoSigintHandler);
+
+ // create a node handle
+ ros::NodeHandle nh;
+
+
+ //create a subscriber
+ ros::Subscriber sub = nh.subscribe("joint_cmd",1, jointCMDCallback);
+
+ // create a loop rate
+ ros::Rate loopRate(10);
+
+
+
+ ROS_INFO("===Launching Poppy node 2===");
+ ros::Publisher jointCmdPublisher = nh.advertise("joint_states", 1);
+ std::vector jointCmdNameArray = {"m1", "m2", "m3", "m4", "m5", "m6"};
+ while (ros::ok())
+ {
+ sensor_msgs::JointState jointCmdMsg;
+ jointCmdMsg.header.stamp = ros::Time::now();
+ jointCmdMsg.header.seq++;
+ jointCmdMsg.position = jointVec;
+ jointCmdMsg.name = jointCmdNameArray;
+ jointCmdPublisher.publish(jointCmdMsg);
+ ROS_INFO("===data published===");
+ ros::spinOnce();
+ loopRate.sleep();
+ }
+ return 0;
+}