diff --git a/catkin_ws/src/ball_tracking/ball_tracking.launch b/catkin_ws/src/ball_tracking/ball_tracking.launch
index e38f327..af5f54c 100644
--- a/catkin_ws/src/ball_tracking/ball_tracking.launch
+++ b/catkin_ws/src/ball_tracking/ball_tracking.launch
@@ -7,5 +7,8 @@
+
+
+
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 30de56b..176f6dc 100755
--- a/catkin_ws/src/ball_tracking/src/aruco_area.py
+++ b/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)
-
- # Initialize the ArUco dictionary
- global arucoDict
+ 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)
- # 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
new file mode 100755
index 0000000..7241896
--- /dev/null
+++ b/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
+
diff --git a/catkin_ws/src/ball_tracking/src/polygon_tester.py b/catkin_ws/src/ball_tracking/src/polygon_tester.py
new file mode 100755
index 0000000..fa2766c
--- /dev/null
+++ b/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()
+
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/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 ccf6adc..747d608 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
)
@@ -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
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/poppy_ros/src/poppy_pong_iv.cpp b/catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp
index bd48a76..cb439ba 100644
--- a/catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp
+++ b/catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp
@@ -1,99 +1,99 @@
-#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;
-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)
-{
- if (sqrt(pow(joint_pos.x, 2) + pow(joint_pos.y, 2)) <= 120)
- {
- geometry_msgs::Twist joint_cmd;
- float q1 = get_q1(joint_pos.x, joint_pos.y);
- float q2 = get_q2(joint_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);
-
-
- // publish the Twist message to the joint_position topic
- _jointPositionPublisher.publish(joint_cmd);
- }
-}
-
-int main(int argc, char** argv)
-{
- //ros::init(argc, argv, "autopilot");
- // create a node called poppy_ros
- ros::init(argc, argv, "poppy_ik");
-
- // create a node handle
- ros::NodeHandle nh;
-
- // create a publisher to joint_position topic
- _jointPositionPublisher = nh.advertise("joint_cmd", 1);
- //create a subscriber
- ros::Subscriber sub = nh.subscribe("position_cmd",1, posCMDCallback);
-
- // 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;
-}
+#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)
+{
+ if (sqrt(pow(joint_pos.x, 2) + pow(joint_pos.y, 2)) <= 120)
+ {
+ if (hitting_need == true)
+ {
+ joint_pos.x += 20;
+ joint_pos.y += 20;
+ }
+ geometry_msgs::Twist joint_cmd;
+ float q1 = get_q1(joint_pos.x, joint_pos.y);
+ float q2 = get_q2(joint_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) {
+ hitting_need = true;
+}
+
+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;
+}