jkhgf

catkin_ws/src/ball_tracking/ball_tracking.launch

@@ -7,5 +7,8 @@
 
     <!-- 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" />
 </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

@@ -3,6 +3,8 @@
 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
@@ -23,17 +25,49 @@ 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)
+
 def image_callback(msg):
-    global 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
+    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()
@@ -42,25 +76,52 @@ def image_callback(msg):
             origin.z = 0
             pub.publish(origin)
 
-            # Optional: Visualize the centroid for debugging
+            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"
+            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)
-            # Also, draw detected markers for visualization
             aruco.drawDetectedMarkers(frame, corners, ids)
 
-    # Display the frame with detected markers and the calculated centroid
     cv2.imshow('Frame', frame)
     cv2.waitKey(1)
 
 def main():
-    global pub
+    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)
-    # Initialize the ArUco dictionary
+    side_pub = rospy.Publisher('/LR_edge_centers', Point, queue_size=10)
-    global arucoDict
+
     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

@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+
+import rospy
+from geometry_msgs.msg import Point
+
+class BallEdgeTrigger:
+    def __init__(self):
+        rospy.init_node('ball_edge_trigger', anonymous=True)
+
+        # Subscribers
+        self.ball_sub = rospy.Subscriber('/ball_coordinates', Point, self.ball_callback)
+        self.edge_sub = rospy.Subscriber('/LR_edge_centers', Point, self.edge_callback)
+
+        # 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
+
+    def edge_callback(self, msg):
+        # Store each edge center with its 'z' value indicating left or right
+        self.edge_centers.append(msg)
+
+    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 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()
+
+if __name__ == '__main__':
+    try:
+        trigger_node = BallEdgeTrigger()
+        trigger_node.run()
+    except rospy.ROSInterruptException:
+        pass
+

catkin_ws/src/ball_tracking/src/polygon_tester.py

@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import rospy
+from sensor_msgs.msg import Image
+from geometry_msgs.msg import Point
+from visualization_msgs.msg import Marker
+from cv_bridge import CvBridge, CvBridgeError
+import cv2
+import numpy as np
+
+class ArucoVisualizer:
+    def __init__(self):
+        self.bridge = CvBridge()
+        self.image_sub = rospy.Subscriber('/camera_image', Image, self.image_callback)
+        self.origin_sub = rospy.Subscriber('/aruco_origin', Point, self.origin_callback)
+        self.edges_sub = rospy.Subscriber('/polygon_edges', Marker, self.edges_callback)
+        
+        self.current_frame = None
+        self.origin_point = None
+        self.edge_points = []
+
+    def origin_callback(self, data):
+        """Callback function for the origin point."""
+        self.origin_point = (int(data.x), int(data.y))
+
+    def edges_callback(self, data):
+        """Callback function for the polygon edges."""
+        self.edge_points = []
+        if data.type == Marker.LINE_STRIP:
+            for p in data.points:
+                self.edge_points.append((int(p.x), int(p.y)))
+    
+    def image_callback(self, data):
+        """Callback function for camera image."""
+        try:
+            cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
+            self.current_frame = cv_image
+            self.draw_visuals()
+        except CvBridgeError as e:
+            print(e)
+
+    def draw_visuals(self):
+        """Draw the origin point and polygon edges on the current frame."""
+        if self.current_frame is not None:
+            frame = self.current_frame.copy()
+            # Draw the origin
+            if self.origin_point is not None:
+                cv2.circle(frame, self.origin_point, 5, (0, 255, 0), -1)
+
+            # Draw the edges
+            if len(self.edge_points) > 1:
+                for i in range(len(self.edge_points) - 1):
+                    cv2.line(frame, self.edge_points[i], self.edge_points[i + 1], (0, 0, 255), 2)
+                # Optionally, close the polygon
+                cv2.line(frame, self.edge_points[-1], self.edge_points[0], (0, 0, 255), 2)
+
+            # Display the frame
+            cv2.imshow('Aruco Visualizer', frame)
+            cv2.waitKey(1)
+
+def main():
+    rospy.init_node('aruco_visualizer', anonymous=True)
+    av = ArucoVisualizer()
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        print("Shutting down")
+    cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+    main()
+

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/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
 )
 
@@ -146,7 +147,7 @@ add_executable(position_ctrl src/position_ctrl.cpp)
 ## 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(readSerial serial ${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

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/poppy_ros/src/poppy_pong_iv.cpp

@@ -1,99 +1,99 @@
-#include "ros/ros.h"
+#include "ros/ros.h"
-#include "geometry_msgs/Twist.h"
+#include "geometry_msgs/Twist.h"
-#include "geometry_msgs/Point.h"
+#include "geometry_msgs/Point.h"
-#include <std_msgs/MultiArrayLayout.h>
+#include <std_msgs/MultiArrayLayout.h>
-#include <std_msgs/MultiArrayDimension.h>
+#include <std_msgs/MultiArrayDimension.h>
-#include <std_msgs/Float32MultiArray.h>
+#include <std_msgs/Float32MultiArray.h>
-#include <array> // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
+#include <array> // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
-#include <cmath> // Inclure la bibliothèque cmath pour atan2
+#include <cmath> // Inclure la bibliothèque cmath pour atan2
-
+
-// Global variables
+// Global variables
-float _fps = 10.0f; // Hz
+float _fps = 10.0f; // Hz
-
+int _nbJoints = 6;
-int _nbJoints = 6;
+float _minJointCmd = 0;
-float _minJointCmd = 0;
+float _maxJointCmd = 1023;
-float _maxJointCmd = 1023;
+float _minJointAngle = -180.0f;
-float _minJointAngle = -180.0f;
+float _maxJointAngle = 180.0f;
-float _maxJointAngle = 180.0f;
+float L1 = 65;
-float L1 = 65;
+float L2 = 55;
-float L2 = 55;
+float angleBase = 100;
-float angleBase = 100;
+bool hitting_need = false;
-ros::Publisher _jointPositionPublisher;
+ros::Publisher _jointPositionPublisher;
-
+
-
+float get_q2(float x, float y)
-
+{
-
+	float q2; // Déclarer un tableau statique de 2 éléments de type float
-float get_q2(float x, float y)
+	q2 = acos((x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2));
-{
+
-	float q2; // Déclarer un tableau statique de 2 éléments de type float
+	return q2;
-	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);
-float get_q1(float x, float y)
+
-{
+	q1 = atan2(y, x) - atan2(L2 * sin(q2), L1 + L2 * cos(q2));
-	float q1;
+
-	float q2 = get_q2(x, y);
+	return q1;
-
+}
-	q1 = atan2(y, x) - atan2(L2 * sin(q2), L1 + L2 * cos(q2));
+
-
+void posCMDCallback(const geometry_msgs::Point& joint_pos)
-	return q1;
+{
-}
+	if (sqrt(pow(joint_pos.x, 2) + pow(joint_pos.y, 2)) <= 120)
-
+	{
-void posCMDCallback(const geometry_msgs::Point& joint_pos)
+		if (hitting_need == true)
-{
+		{
-	if (sqrt(pow(joint_pos.x, 2) + pow(joint_pos.y, 2)) <= 120)
+			joint_pos.x += 20;
-	{
+			joint_pos.y += 20;
-		geometry_msgs::Twist joint_cmd;
+		}
-		float q1 = get_q1(joint_pos.x, joint_pos.y);
+		geometry_msgs::Twist joint_cmd;
-		float q2 = get_q2(joint_pos.x, joint_pos.y);
+		float q1 = get_q1(joint_pos.x, joint_pos.y);
-		// stores them into a msg
+		float q2 = get_q2(joint_pos.x, joint_pos.y);
-		joint_cmd.linear.x = q1*(180.0f/3.141592f);
+		// stores them into a msg
-		joint_cmd.linear.y = angleBase;
+		joint_cmd.linear.x = q1*(180.0f/3.141592f);
-		joint_cmd.linear.z = q2*(180.0f/3.141592f);
+		joint_cmd.linear.y = angleBase;
-		joint_cmd.angular.x = (-q1-q2)*(180.0f/3.141592f);
+		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
+		// publish the Twist message to the joint_position topic
-		_jointPositionPublisher.publish(joint_cmd);
+		_jointPositionPublisher.publish(joint_cmd);
-	}
+	}
-}
+}
-
+
-int main(int argc, char** argv)
+void hittingCommand(const geometry_msgs::Point& trigger_signal) {
-{
+	hitting_need = true;
-	//ros::init(argc, argv, "autopilot");
+}
-	// create a node called poppy_ros
+
-	ros::init(argc, argv, "poppy_ik");
+int main(int argc, char** argv)
-
+{
-	// create a node handle
+	// create a node called poppy_ik
-	ros::NodeHandle nh;
+	ros::init(argc, argv, "poppy_ik");
-
+
-	// create a publisher to joint_position topic
+	// create a node handle
-	_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_cmd", 1);
+	ros::NodeHandle nh;
-	//create a subscriber
+
-	ros::Subscriber sub = nh.subscribe("position_cmd",1, posCMDCallback);
+	// create a publisher to joint_cmd topic
-	
+	_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_cmd", 1);
-	// create a loop rate
+	// create a subscriber to position_cmd
-	ros::Rate loopRate(_fps);
+	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
+	ROS_INFO("===Launching Poppy node===");
-	while(ros::ok())
+	
-	{
+	// 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();
+		// 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();
+		// sleep the right amout of time to comply with _fps 
-	}
+		loopRate.sleep();
-	
+	}
-	
+	
-	return 0;
+	return 0;
-}
+}