Final Version

catkin_ws/src/CMakeLists.txt

@@ -1 +0,0 @@
-/opt/ros/noetic/share/catkin/cmake/toplevel.cmake

catkin_ws/src/ball_tracking/ball_tracking.launch

@@ -8,9 +8,6 @@
     <!-- Ball Tracking Node -->
     <node name="tracker_aruco" pkg="ball_tracking" type="tracker_aruco.py" output="screen" />
     
-    <!-- Ball Tracking Node -->
-    <node name="tracker_aruco_idk" pkg="ball_tracking" type="tracker_aruco_idk.py" output="screen" />
-    
     <!-- Ball Tracking Node -->
     <node name="paddle_action" pkg="ball_tracking" type="paddle_action.py" output="screen" />
 </launch>

catkin_ws/src/ball_tracking/ball_tracking_exp.launch

@@ -0,0 +1,20 @@
+<launch>
+    <!-- ArUco Tag Detection Node -->
+    <node name="camera_publisher" pkg="ball_tracking" type="camera_publisher.py" output="screen" />
+
+    <!-- ArUco Tag Detection Node -->
+    <node name="aruco_area" pkg="ball_tracking" type="aruco_area.py" output="screen" />
+
+    <!-- Ball Tracking Node -->
+    <node name="tracker_aruco" pkg="ball_tracking" type="tracker_aruco.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="paddle_action" pkg="ball_tracking" type="paddle_action.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="ball_prediction" pkg="ball_tracking" type="ball_prediction.py" output="screen" />
+    
+    <!-- Ball Tracking Node -->
+    <node name="prediction_monitor" pkg="ball_tracking" type="prediction_monitor.py" output="screen" />
+</launch>
+

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.scale.x = 0.02
-            marker.color = ColorRGBA(1.0, 0.0, 0.0, 1.0)  # Color
+            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)
-            
+            if sorted_centers:
-            # Close the polygon by adding the first point again to connect the last and first midpoints
-            if marker_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)
+    marker_pub = rospy.Publisher('/polygon_edges', Marker, queue_size=10)
-    
+    side_pub = rospy.Publisher('/LR_edge_centers', Point, queue_size=10)
-    # Initialize the ArUco dictionary
+    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()

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
+

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
+

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
+        # Publishers
-        self.trigger_pub = rospy.Publisher('/trigger', Point, queue_size=10)  # Change to Point
+        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()

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()
+

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')

catkin_ws/src/ball_tracking/src/tracker_aruco_idk.py

@@ -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()
-

catkin_ws/src/ball_tracking/src/trackernew.py

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

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}

catkin_ws/src/joystick_ros/joystick_ros.launch

@@ -0,0 +1,15 @@
+<!-- joystick_ros.launch -->
+<launch>
+    <!-- Launch joystick_ros -->
+    <!--include file="$(find joystick_ros)/joystick_ros.launch" /-->
+
+    <!-- Launch parser -->
+    <node name="parser" pkg="joystick_ros" type="parser" />
+    
+    <!-- Launch position_ctrl -->
+    <node name="position_ctrl" pkg="joystick_ros" type="position_ctrl" />
+    
+    <!-- Launch readSerial -->
+    <node name="readSerial" pkg="joystick_ros" type="readSerial" />
+</launch>
+

catkin_ws/src/joystick_ros/package.xml

@@ -52,12 +52,15 @@
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
+  <build_depend>serial</build_depend>
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
+  <build_export_depend>serial</build_export_depend>
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
   <exec_depend>std_msgs</exec_depend>
+  <exec_depend>serial</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

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<std_msgs::Float32MultiArray>("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<std_msgs::Float32MultiArray>("axis_values", 10);
-
     // Spin ROS node
     ros::spin();
 
     return 0;
 }
 
+

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 <serial_port> <baud_rate>");
-        return 1;
-    }
 
-    std::string port = "COM14";
+    std::string port = "/dev/ttyACM0"; // Get serial port from command-line argument
-    uint32_t baud_rate = 9600;
+    uint32_t baud_rate = 9600; // Convert string to integer
 
     readSerial(port, baud_rate);
 

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)

catkin_ws/src/poppy_ros/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>poppy_ros</name>
+  <version>0.0.0</version>
+  <description>The poppy_ros package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="lucas@todo.todo">lucas</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/poppy_ros</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>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_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>

catkin_ws/src/poppy_ros/poppy_ros.launch

@@ -0,0 +1,12 @@
+<!-- poppy_launch.launch -->
+<launch>
+    <!-- Launch poppy_ros -->
+    <!--include file="$(find poppy_ros)/poppy_ros.launch" /-->
+
+    <!-- Launch poppy_pong_iv -->
+    <node name="poppy_ik" pkg="poppy_ros" type="poppy_pong_iv" />
+    
+    <!-- Launch poppy_ros -->
+    <node name="poppy_ros" pkg="poppy_ros" type="poppy_ros" />
+</launch>
+

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 <std_msgs/MultiArrayLayout.h>
+#include <std_msgs/MultiArrayDimension.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <array> // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
+#include <cmath> // 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<geometry_msgs::Twist>("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;
+}
+

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<uint16_t> 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<geometry_msgs::Twist>("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<uint16_t> 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;
+}

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<geometry_msgs::Twist>("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;
+}

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 <std_msgs/MultiArrayLayout.h>
+#include <std_msgs/MultiArrayDimension.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <array> // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
+#include <cmath> // 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<geometry_msgs::Point>("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;
+}

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<double> 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<sensor_msgs::JointState>("joint_states", 1);
+	std::vector<std::string> 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;
+}