diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c18dd8d --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__/ diff --git a/cartographer_slam/maps/sim_map.pgm b/cartographer_slam/maps/sim_map.pgm index 29a5525..0eb678d 100644 Binary files a/cartographer_slam/maps/sim_map.pgm and b/cartographer_slam/maps/sim_map.pgm differ diff --git a/cartographer_slam/maps/sim_map_orig.pgm b/cartographer_slam/maps/sim_map_orig.pgm new file mode 100644 index 0000000..29a5525 Binary files /dev/null and b/cartographer_slam/maps/sim_map_orig.pgm differ diff --git a/localization_server/config/amcl_config.yaml b/localization_server/config/amcl_config.yaml index 09f0e04..27208e1 100644 --- a/localization_server/config/amcl_config.yaml +++ b/localization_server/config/amcl_config.yaml @@ -6,7 +6,7 @@ amcl: alpha3: 0.2 alpha4: 0.2 alpha5: 0.2 - base_frame_id: "robot_base_footprint" + base_frame_id: "robot_base_link" beam_skip_distance: 0.5 beam_skip_error_threshold: 0.9 beam_skip_threshold: 0.3 @@ -15,7 +15,7 @@ amcl: lambda_short: 0.1 laser_likelihood_max_dist: 2.0 laser_max_range: 100.0 - laser_min_range: -1.0 + laser_min_range: 0.2 laser_model_type: "likelihood_field" max_beams: 60 max_particles: 8000 @@ -32,12 +32,13 @@ amcl: tf_broadcast: true transform_tolerance: 1.0 update_min_a: 0.2 - update_min_d: 0.25 + update_min_d: 0.2 z_hit: 0.5 z_max: 0.05 z_rand: 0.5 z_short: 0.05 - set_initial_pose: true + scan_topic: scan + set_initial_pose: false initial_pose: x: 0.0 y: 0.0 diff --git a/path_planner_server/config/controller.yaml b/path_planner_server/config/controller.yaml index 36a40c6..472c904 100644 --- a/path_planner_server/config/controller.yaml +++ b/path_planner_server/config/controller.yaml @@ -30,7 +30,7 @@ controller_server: min_vel_y: 0.0 max_vel_x: 0.20 max_vel_y: 0.0 - max_vel_theta: 0.5 + max_vel_theta: 0.3 min_speed_xy: 0.0 max_speed_xy: 0.25 min_speed_theta: 0.0 diff --git a/robot_firmware/launch/table_detection.launch.py b/robot_firmware/launch/table_detection.launch.py new file mode 100644 index 0000000..cd90daf --- /dev/null +++ b/robot_firmware/launch/table_detection.launch.py @@ -0,0 +1,38 @@ +import os +from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + + package_name = "robot_firmware" + + + # RVIZ configuration file + rviz_file = "rvis.rviz" + rviz_config_dir = os.path.join(get_package_share_directory(package_name), "rviz", rviz_file) + rviz_config_dir = "/home/user/ros2_ws/src/rvis.rviz" + + return LaunchDescription([ + + Node( + package=package_name, + executable='table_detection', + name='table_detection', + output='screen', + parameters=[{'use_sim_time': True}], + ), + + Node( + package="rviz2", + executable="rviz2", + output="screen", + parameters=[{ + "use_sim_time": True, + }], + arguments=[ + "-d ", rviz_config_dir + ] + ) + + ]) diff --git a/robot_firmware/robot_firmware/action.py b/robot_firmware/robot_firmware/action.py new file mode 100644 index 0000000..99401d7 --- /dev/null +++ b/robot_firmware/robot_firmware/action.py @@ -0,0 +1,66 @@ +#! /usr/bin/env python3 + +import rclpy +from rclpy.action import ActionServer +from rclpy.node import Node + +from robot_firmware_interfaces.action import AproachTable # import the action message +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + + +class SimpleActionServer(Node): + + def __init__(self): + super().__init__('main_node') + self._action_server = ActionServer( + self, + AproachTable, + 'aproach_table', + self.execute_callback, + callback_group=MutuallyExclusiveCallbackGroup() + ) + + def execute_callback(self, goal_handle): + self.get_logger().info('Executing goal...') + + # Perform the desired action here + # ... + + # Check if the action has been canceled + if goal_handle.is_cancel_requested: + goal_handle.canceled() + self.get_logger().info('Goal canceled') + return + + # Check if the action has been preempted + # if goal_handle.is_preempt_requested: + # goal_handle.abort() + # self.get_logger().info('Goal preempted') + # return + + # Set the result of the action + result = AproachTable.Result() + result.result = 1 + + goal_handle.succeed() + return result + + self.get_logger().info('Goal completed') + +def main(args=None): + rclpy.init(args=args) + simple_action_server = SimpleActionServer() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(simple_action_server) + + executor.spin() + executor.shutdown() + + rclpy.shutdown() + +if __name__ == '__main__': + main() + + diff --git a/robot_firmware/robot_firmware/main_node.py b/robot_firmware/robot_firmware/main_node.py index b1231b0..397f8da 100644 --- a/robot_firmware/robot_firmware/main_node.py +++ b/robot_firmware/robot_firmware/main_node.py @@ -3,51 +3,214 @@ import rclpy from rclpy.action import ActionServer from rclpy.node import Node +from rclpy.duration import Duration -from robot_firmware_interfaces.action import Empty # import the action message +from rclpy.action import ActionClient +from robot_firmware_interfaces.action import AproachTable # import the action message +from geometry_msgs.msg import PoseStamped +from action_msgs.msg import GoalStatus -class SimpleActionServer(Node): +from nav2_simple_commander.robot_navigator import BasicNavigator, TaskResult +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +from tf_transformations import quaternion_from_euler + +# ros2 run robot_firmware main_node --ros-args -p use_sim_time:=true + +# Home position +home_position = { + # [x, y, theta - angle to rotate when robot is under table] + "home_1": [7.25, -2.5, -1.57, 0.0], +} + +# Shelf positions for picking +trash_table_position = { + # [x, y, theta - angle to rotate when robot is under table] + "table_1": [-0.25, 1.20, 0.0, -1.57], +} + + +class MainNode(Node): def __init__(self): super().__init__('main_node') - self._action_server = ActionServer( - self, - Empty, - 'remove_trash_table', - self.execute_callback) - - def execute_callback(self, goal_handle): - self.get_logger().info('Executing goal...') - - # Perform the desired action here - # ... - - # Check if the action has been canceled - if goal_handle.is_cancel_requested: - goal_handle.canceled() - self.get_logger().info('Goal canceled') + + self.navigator = BasicNavigator() + self.state = "" + + self.get_logger().info('Waiting for navigattion to start...') + self.navigator.waitUntilNav2Active() + + # Create action client + self.action_cli = ActionClient(self, AproachTable, 'aproach_table') + + # Create dummy timer + self.timer = self.create_timer(1.0, self.timer_clb, callback_group=MutuallyExclusiveCallbackGroup()) + self.action_status = False + + # self.get_logger().info('Initializing robot pose...') + # self.set_initial_position() + + self.get_logger().info('Main node started') + + + def timer_clb(self): + self.timer.cancel() + self.action_status = False + + # Step 1 - go to known location where table might be + table_loc = trash_table_position['table_1'] + result = self.go_to_pose(table_loc[0], table_loc[1], table_loc[2]) + if result != TaskResult.SUCCEEDED: + print(f"Step 1 - go to known location where table might be: FAILED {result}") + # TODO process failed task + return + print("Step 1 - go to known location where table might be: DONE") + + + # Step 2 - trye to approach table + (status, result) = self.aproach_table(dummy_aproach=True) + if status != GoalStatus.STATUS_SUCCEEDED : + print(f"action client failed: {status}") + # TODO process failed task + return + + # 0 - success, 1 - table not found, 2 - failed approach + if result.result: + print(f"Action cliented returned: {result.result}") + # TODO process failed task - go home + return + + print(f"action server finished with result {result.result}") + + + # Step 3 - Rotate robot + result = self.rotate_robot(yaw=table_loc[3]) + if not result: + print(f"Step 3 - Rotate robot: FAILED") + # TODO process failed task + return + self.create_rate(1/(5*abs(table_loc[3]))).sleep() + print(f"Step 3 - Rotate robot: SUCCESS") + + # Step 4 - Lift elevator + # Step 4.1 - Change footprint + + + # Step 5 - Move table to back room + + + # Step 6 - Down elevator + # Step 6.1 - Change footprint + + + # Step 7 - Get out of table + + + # Step 7 - Go to home position + home_loc = home_position['home_1'] + result = self.go_to_pose(home_loc[0], home_loc[1], home_loc[2]) + if result != TaskResult.SUCCEEDED: + print(f"Step 7 - go to home position: FAILED {result}") + # TODO process failed task return + print("Step 7 - go to home positio: DONE") + - # Check if the action has been preempted - # if goal_handle.is_preempt_requested: - # goal_handle.abort() - # self.get_logger().info('Goal preempted') - # return - # Set the result of the action - result = Empty.Result() - result.result = True - goal_handle.succeed() + + + + + def set_initial_position(self): + print("Set initial position") + # Set your demo's initial pose + initial_pose = PoseStamped() + initial_pose.header.frame_id = 'map' + initial_pose.header.stamp = self.get_clock().now().to_msg() + initial_pose.pose.position.x = 0.0 + initial_pose.pose.position.y = 0.0 + initial_pose.pose.orientation.x = 0.0 + initial_pose.pose.orientation.y = 0.0 + initial_pose.pose.orientation.z = 0.0 + initial_pose.pose.orientation.w = 1.0 + + self.navigator.setInitialPose(initial_pose) + + def go_to_pose(self, x, y, yaw): + shelf_item_pose = PoseStamped() + shelf_item_pose.header.frame_id = 'map' + shelf_item_pose.header.stamp = self.get_clock().now().to_msg() + shelf_item_pose.pose.position.x = x + shelf_item_pose.pose.position.y = y + + q = quaternion_from_euler(0, 0, yaw) + + shelf_item_pose.pose.orientation.x = q[0] + shelf_item_pose.pose.orientation.y = q[1] + shelf_item_pose.pose.orientation.z = q[2] + shelf_item_pose.pose.orientation.w = q[3] + print(f'Received request for item picking at {shelf_item_pose.pose}.') + self.navigator.goToPose(shelf_item_pose) + + # Do something during our route + # (e.x. queue up future tasks or detect person for fine-tuned positioning) + # Simply print information for workers on the robot's ETA for the demonstation + i = 0 + while not self.navigator.isTaskComplete(): + i += 1 + feedback = self.navigator.getFeedback() + if feedback and i % 20 == 0: + # print( + # 'Estimated time of arrival at ' + # + str(shelf_item_pose.pose) + # + ' for worker: ' + # + '{0:.0f}'.format( + # Duration.from_msg(feedback.estimated_time_remaining).nanoseconds + # / 1e9 + # ) + # + ' seconds.' + # ) + print("Moving") + + result = self.navigator.getResult() return result + + def aproach_table(self, dummy_aproach): + goal_msg = AproachTable.Goal() + goal_msg.dummy_aproach = dummy_aproach + + available = self.action_cli.wait_for_server(timeout_sec=3) + if not available: + return(-1, None) + + res = self.action_cli.send_goal(goal_msg) + status = res.status + result = res.result + return (status, result) + + + + def rotate_robot(self, yaw) -> bool: + # return True if SUCCESS else False + status = self.navigator.spin(spin_dist=yaw) + return status - self.get_logger().info('Goal completed') def main(args=None): rclpy.init(args=args) - simple_action_server = SimpleActionServer() - rclpy.spin(simple_action_server) + main_node = MainNode() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(main_node) + + executor.spin() + executor.shutdown() + + rclpy.spin(main_node) rclpy.shutdown() if __name__ == '__main__': diff --git a/robot_firmware/robot_firmware/table_detection.py b/robot_firmware/robot_firmware/table_detection.py new file mode 100644 index 0000000..1b4a86c --- /dev/null +++ b/robot_firmware/robot_firmware/table_detection.py @@ -0,0 +1,661 @@ + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import math, time +from statistics import mean + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +from rclpy.action import ActionServer + +from robot_firmware_interfaces.action import AproachTable # import the action message +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +# ros2 run robot_firmware table_detection --ros-args -p use_sim_time:=true + +# ros2 action send_goal /aproach_table robot_firmware_interfaces/action/AproachTable "dummy_aproach: true" + + +class TableDetectionNode(Node): + def __init__(self): + super().__init__('table_detection_node') + self.laser_scan_subscription = self.create_subscription( + LaserScan, + 'scan', + self.scan_callback, + 3 + ) + self.laser_scan_subscription # prevent unused variable warning + self.br = tf2_ros.TransformBroadcaster(self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create action server for approaching table + self.action_server = ActionServer( + self, + AproachTable, + 'aproach_table', + self.execute_action_callback, + callback_group=MutuallyExclusiveCallbackGroup() + ) + + # Timer to publish tf to approach table + self.timer_tf = self.create_timer(0.1, self.timer_tf_clb, callback_group=MutuallyExclusiveCallbackGroup()) + + + + self.publish_table_tf = False # defult False + self.is_table = False + + + def execute_action_callback(self, goal_handle): + # Enable table tf publishing + self.publish_table_tf = True + + self.create_rate(1/2.0).sleep() # Sleep 2 sec + + # 0 - success, 1 - table not found, 2 - failed approach + result = AproachTable.Result() + + # self.publish_table_tf = False + # goal_handle.succeed() + # return result + + dummy_aproach = goal_handle.request.dummy_aproach + + if not self.is_table: + print("Table not found") + result.result = 1 + result.msg = "Table not found" + goal_handle.succeed() + self.publish_table_tf = False + return result + + # try to reach pre_table_link + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + scale_forward_speed = 0.15 + scale_rotation = 1.30 + + distance = 99.0 + # angle = 0.0 + + msg = Twist() + while(distance > 0.03): + x1, y1 = self.read_tf(target_frame=target_frame, ref_frame=ref_frame) + if x1 == None: + continue + distance = math.sqrt(x1**2 + y1**2) + angle = math.atan2(y1, x1) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.25 * angle / abs(angle) + else: + msg.linear.x = scale_forward_speed + sign = angle / abs(angle) + msg.angular.z = sign * scale_rotation * abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.1) + + print("Reached pre table") + # Stop robot + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + #self.create_rate(1.0).sleep() + + + # Turn to "back_table_frame" + #distance = 99.0 + angle = 99.0 + + target_frame = 'robot_base_link' + ref_frame = 'back_table_frame' + + msg = Twist() + while(abs(angle) > 0.087): # 5 degrees + x1, y1 = self.read_tf(target_frame=target_frame, ref_frame=ref_frame) + if x1 == None: + continue + # distance = math.sqrt(x1**2 + y1**2) + angle = math.atan2(y1, x1) + + # print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + msg.angular.z = 0.20 * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.1) + + print("Turned to back table link") + # Stop robot + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + #self.create_rate(1.0).sleep() + + # CHeck if there is still table + if not self.is_table: + print("Table not found after in fro of table") + result.result = 1 + result.msg = "Table not found" + goal_handle.succeed() + self.publish_table_tf = False + return result + + # Table is found + if dummy_aproach: + print("Table on robot - dummy approach") + result.result = 0 + result.msg = "Table on robot - dummy approach" + goal_handle.succeed() + self.publish_table_tf = False + return result + + # Go to "back_table_frame" + distance = 99.0 + #angle = 99.0 + + target_frame = 'robot_base_link' + ref_frame = 'back_table_frame' + + msg = Twist() + while(distance > 0.4): + # Stop publish whgen uder, so no mistkaes + if distance < 0.8: + self.publish_table_tf = False + x1, y1 = self.read_tf(target_frame=target_frame, ref_frame=ref_frame) + if x1 == None: + continue + distance = math.sqrt(x1**2 + y1**2) + angle = math.atan2(y1, x1) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + msg.linear.x = scale_forward_speed + msg.angular.z = scale_rotation * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.1) + + + print("Robot is under table") + + + # self.get_logger().error(f"Dummy approach is not implemented yet") + # Stop robot + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + + result.result = 0 + result.msg = "Robot is under table" + goal_handle.succeed() + self.publish_table_tf = False + return result + + + def timer_tf_clb(self): + # NOTE - timer execution might be longer than callback interval + + if not self.is_table: + return + + # Is table + frame_name = [f"leg_{i}" for i in range(4)] + + # Get first leg pos + x1, y1 = self.read_tf(target_frame="odom", ref_frame="leg_0") #map + x2, y2 = self.read_tf(target_frame="odom", ref_frame="leg_1") #map + + if x1 == None or x2 == None: + return + + dy = y2 - y1 + dx = x2 - x1 + if dy == 0: + dy = 1e-6 + if dx == 0: + dx = 1e-6 + + x_mid = (x1 + x2)/2 + y_mid = (y1 + y2)/2 + slope = math.atan2(dy, dx) + self.publish_tf(x_mid, y_mid, theta=slope, parent_frame = "odom", frame_name="front_table_frame") #map + + # Get second leg pos + x1, y1 = self.read_tf(target_frame="odom", ref_frame="leg_2") #map + x2, y2 = self.read_tf(target_frame="odom", ref_frame="leg_3") #map + + if x1 == None or x2 == None: + return -1 + + dy = y2 - y1 + dx = x2 - x1 + if dy == 0: + dy = 1e-6 + if dx == 0: + dx = 1e-6 + + x_mid = (x1 + x2)/2 + y_mid = (y1 + y2)/2 + slope = math.atan2(dy, dx) + self.publish_tf(x_mid, y_mid, theta=slope, parent_frame = "odom", frame_name="back_table_frame") #map + + # Publish pre table frame + x1, y1 = self.read_tf(target_frame="front_table_frame", ref_frame="robot_base_link") + if x1 == None: + return + # Publish extra frame for robot before going under the table + d_y = 0.4 + y = -d_y if y1 <= 0 else d_y + + self.publish_tf(0.0, y, theta=0.0, parent_frame = "front_table_frame", frame_name="pre_table_frame") + + + + + def scan_callback(self, msg): + + if not self.publish_table_tf: + self.is_table = False + return + # Get the range values from the laser scan message + ranges = msg.ranges + + # Define the minimum and maximum distances for table legs + min_range = 0.1 # Minimum range value for table detection + max_range = 2.5 # Maximum range value for table detection + + max_two_point_dist = 0.07 # Maximum distance between 2 laser reading to be in 1 group + min_group_size = 4 + + table_leg_size = 0.22 + + table_x_size = 0.66 + table_y_size = 0.75 + table_diagnol = math.sqrt(table_x_size**2 + table_y_size**2) + table_size_tolerance = 0.1 + + #group = [] + prev_range = msg.ranges[0] + scans_too_big = 0 + + # Filter parameters + min_distance = 0.1 # Minimum distance to keep + max_distance = 2.5 # Maximum distance to keep + + # Create a new LaserScan message for filtered data + filtered_msg = LaserScan() + filtered_msg.header = msg.header + filtered_msg.angle_min = msg.angle_min + filtered_msg.angle_max = msg.angle_max + filtered_msg.angle_increment = msg.angle_increment + filtered_msg.time_increment = msg.time_increment + filtered_msg.scan_time = msg.scan_time + filtered_msg.range_min = msg.range_min + filtered_msg.range_max = msg.range_max + + # Filter the ranges based on distance + filtered_ranges = [] + for range in msg.ranges: + if min_distance <= range <= max_distance: + filtered_ranges.append(range) + else: + filtered_ranges.append(0.0) # Set invalid ranges to 0.0 + + # Update the filtered ranges in the new LaserScan message + filtered_msg.ranges = filtered_ranges + + + # Split in groups or objects + splitted_groups = [] + self.split_in_groups(filtered_msg, splitted_groups, max_two_point_dist) + + + # Remove too small groups/noise + filtered_groups_by_size = [] + self.filter_groups_by_size(splitted_groups, filtered_groups_by_size, min_group_size) + + + # Filter groups by shape + filtered_groups = [] + for i, group in enumerate(filtered_groups_by_size): + #print(list(zip(*group))) + if len(list(zip(*group))) == 0: + continue + min_dist = min(list(zip(*group))[0]) + max_dist = max(list(zip(*group))[0]) + delta_dist = abs(max_dist - min_dist) + start_angle = list(zip(*group))[1][0] + start_dist = list(zip(*group))[0][0] + end_angle = list(zip(*group))[1][-1] + end_dist = list(zip(*group))[0][-1] + dist_between_start_end = self.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) + + #print(f"Delta {delta_dist:.2f} {dist_between_start_end:.2f}") + + if dist_between_start_end <= table_leg_size and \ + delta_dist <= table_leg_size: + filtered_groups.append(group) + + + table = False + class TableLeg: + distance = 0 + angle = 0 + + front_left_leg = TableLeg() + back_left_leg = TableLeg() + back_right_leg = TableLeg() + front_right_leg = TableLeg() + + if len(filtered_groups) < 4: + print("Too less groups") + + # Calculate distacne betwen groups + for i, group in enumerate(filtered_groups): + # Check if there are at least 3 legs left + if not len(filtered_groups) - i >= 3: + #print("NO TABLE") + break + if len(list(zip(*group))) == 0: + continue + first_leg = TableLeg() + first_leg.distance = mean(list(zip(*group))[0]) + first_leg.angle = mean(list(zip(*group))[1]) + possible_table_leg_distances = [] + table_legs = [] + table_legs.append(first_leg) + + for j, group_target in enumerate(filtered_groups): + if j <= i: + continue + possible_leg = TableLeg() + possible_leg.distance = mean(list(zip(*group_target))[0]) + possible_leg.angle = mean(list(zip(*group_target))[1]) + dist_between_start_end = self.distance_between_polar_coordinates( + first_leg.distance, + first_leg.angle, + possible_leg.distance, + possible_leg.angle) + possible_table_leg_distances.append(dist_between_start_end) + table_legs.append(possible_leg) + + + # Analyze distances + #print(dst) + #print([f"{i:.2f}" for i in possible_table_leg_distances]) + + #print(len(possible_table_leg_distances), len(table_legs)) + + leg_x = False + leg_y = False + leg_middle = False + leg_idx = [] + for i, d in enumerate(possible_table_leg_distances): + x_size = abs(d - table_x_size) + y_size = abs(d - table_y_size) + diagnol_size = abs(d - table_diagnol) + + #print("Table errors:") + #print(x_size, y_size, diagnol_size) + + if x_size < table_size_tolerance and \ + y_size < table_size_tolerance: + if x_size < y_size and not leg_x: + #print(f"leg_x 1: {x_size}") + leg_x = True + leg_idx.append(i) + elif not leg_y: + #print(f"leg_y 1: {y_size}") + leg_y = True + leg_idx.append(i) + elif x_size < table_size_tolerance and not leg_x: + #print(f"leg_x 2: {x_size}") + leg_x = True + leg_idx.append(i) + elif y_size < table_size_tolerance and not leg_y: + #print(f"leg_y 2: {y_size}") + leg_y = True + leg_idx.append(i) + if diagnol_size < table_size_tolerance and \ + (leg_x or leg_y): + #print("leg_mid") + leg_middle = True + leg_idx.append(i) + if leg_x and leg_y and leg_middle: + break + + if leg_x and leg_y and leg_middle: + table = True + #print([f"{d:.2f} " for d in possible_table_leg_distances]) + break + if table: + #self.get_logger().info("TABLE") + # info = f"{i}" for i in leg_idx + #self.get_logger().info("".join(map(str, leg_idx))) + front_left_leg = table_legs[0] + back_left_leg = table_legs[leg_idx[0]+1] + back_right_leg = table_legs[leg_idx[1]+1] + front_right_leg = table_legs[leg_idx[2]+1] + else: + self.get_logger().info("NO TABLE") + + if table: + legs = [front_left_leg, back_left_leg, back_right_leg, front_right_leg] + # Sort legs by length to robot + legs.sort(key=lambda x:x.distance) + #self.get_logger().info("__________") + legs_x = 0 + legs_y = 0 + for i, leg in enumerate(legs): + x = leg.distance * math.cos(leg.angle) + y = leg.distance * math.sin(leg.angle) + legs_x += x + legs_y +=y + self.publish_tf(x, y, frame_name=f"leg_{i}") + # self.get_logger().info(f"angle: {leg.angle:.2f}") + # self.get_logger().info(f"dist: {leg.distance:.2f}") + + + # mean_angle = mean([i.angle for i in legs]) + # mean_distance = mean([i.distance for i in legs]) + # x = mean_distance * math.cos(mean_angle) + # y = mean_distance * math.sin(mean_angle) + + self.publish_tf(legs_x/4, legs_y/4) + + legs.sort(key=lambda x:x.distance) + + first_leg = legs[0] + second_leg = legs[1] + first_leg_x = first_leg.distance * math.cos(first_leg.angle) + first_leg_y = first_leg.distance * math.sin(first_leg.angle) + second_leg_x = second_leg.distance * math.cos(second_leg.angle) + second_leg_y = second_leg.distance * math.sin(second_leg.angle) + + x_mid = (first_leg_x + second_leg_x)/2 + y_mid = (first_leg_y + second_leg_y)/2 + slope = second_leg_y - first_leg_y / second_leg_x - first_leg_x + #self.publish_tf(x_mid, y_mid, theta=slope, frame_name="pre_table_frame") + + self.is_table = True + + else: + self.is_table = False + + + + def publish_tf(self, x, y, theta=0, parent_frame = "robot_front_laser_base_link", frame_name = "table_frame"): + # TF broadcaster + transfor_msg = TransformStamped() + # transfor_msg.header.stamp = self.get_clock().now().to_msg() + transfor_msg.header.stamp = self.get_clock().now().to_msg() + # xx = int(transfor_msg.header.stamp.sec) + # self.get_logger().info(f"{transfor_msg.header.stamp}") + transfor_msg.header.frame_id = parent_frame + transfor_msg.child_frame_id = frame_name + + # Find middle point + transfor_msg.transform.translation.x = x + transfor_msg.transform.translation.y = y + transfor_msg.transform.translation.z = 0.0 + + # Rotation + q = quaternion_from_euler(0, 0, theta) + + transfor_msg.transform.rotation.x = q[0] + transfor_msg.transform.rotation.y = q[1] + transfor_msg.transform.rotation.z = q[2] + transfor_msg.transform.rotation.w = q[3] + + self.br.sendTransform(transfor_msg) + + + def read_tf(self, target_frame, ref_frame, fixed_frame="odom"): + try: + t = self.tf_buffer.lookup_transform_full( + target_frame=target_frame, + target_time=rclpy.time.Time(), + source_frame=ref_frame, + source_time=rclpy.time.Time(), + fixed_frame=fixed_frame, + timeout=rclpy.duration.Duration(seconds=0.1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().warn( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + return (None, None) + + return (t.transform.translation.x, t.transform.translation.y) + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + + @staticmethod + def split_in_groups(msg, groups, max_two_point_dist): + prev_range = msg.ranges[0] + prev_idx = msg.angle_min + + max_two_point_dist = max_two_point_dist + group = [] + + # Iterate through the laser scan readings + for i in range(1, len(msg.ranges)): + if msg.ranges[i] == 0.0: + continue + curr_range = msg.ranges[i] + curr_idx = msg.angle_min + msg.angle_increment * i + + # Check if the current reading is part of the same object + d = TableDetectionNode.distance_between_polar_coordinates( + prev_range, + prev_idx, + curr_range, + curr_idx + ) + if abs(d) < max_two_point_dist: + group.append((curr_range, curr_idx)) + else: + # Add the current group to the list of groups + if len(group) >= 0: + groups.append(group) + + # Start a new group with the current reading + group = [(curr_range, curr_idx)] + + # Update the previous range + prev_range = curr_range + prev_idx = curr_idx + + # Add the last group to the list of groups + if len(group) >= 0: + groups.append(group) + + @staticmethod + def filter_groups_by_size(in_groups, out_groups, min_group_size): + # remove too small group by thershold value + for i, group in enumerate(in_groups): + if len(group) >= min_group_size: + out_groups.append(group) + + @staticmethod + def merge_groups_by_size(in_groups, out_groups, max_two_point_dist): + for i, group in enumerate(in_groups): + if i == 0: + out_groups.append(group) + continue + start_angle = min(list(zip(*group))[1]) + start_dist = list(zip(*group))[0][0] + # Debug + #print(f"Idx {i}, group len: {len(group)}") + + end_angle = max(list(zip(*out_groups[-1]))[1]) + end_dist = list(zip(*out_groups[-1]))[0][-1] + dist_between_start_end = TableDetectionNode.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) + + if dist_between_start_end <= max_two_point_dist+0.01: + #print("Append to prev") + + out_groups[-1].extend(group) + else: + out_groups.append(group) + + @staticmethod + def distance_between_polar_coordinates(r1, theta1, r2, theta2): + # Convert polar coordinates to Cartesian coordinates + x1 = r1 * math.cos(theta1) + y1 = r1 * math.sin(theta1) + x2 = r2 * math.cos(theta2) + y2 = r2 * math.sin(theta2) + + # Calculate the distance between the Cartesian coordinates + distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) + + return distance + + + + + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TableDetectionNode() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(table_detection_node) + + executor.spin() + executor.shutdown() + + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/robot_firmware/robot_firmware/tf2_listener copy 1.py b/robot_firmware/robot_firmware/tf2_listener copy 1.py new file mode 100644 index 0000000..eda41bb --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener copy 1.py @@ -0,0 +1,146 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import math +from statistics import mean +import time + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +class TfListener(Node): + def __init__(self): + super().__init__('tf_listener') + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + #self.timer = self.create_timer(1, self.timer_clb) + + def timer_clb(self): + #self.timer.cancel() + print("timer clb") + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + + scale_forward_speed = 0.1 + scale_rotation = 0.8 + + distance = 99.0 + angle = 0.0 + + msg = Twist() + + #rclpy.spin_once(self.tf_listener) + + while(distance > 0.1): + try: + # t = self.tf_buffer.lookup_transform_full( + # #print(self.get_clock().now()) + # target_frame=target_frame, + # target_time=rclpy.duration.Duration(seconds=0.0), + # source_frame=ref_frame, + # source_time=rclpy.duration.Duration(seconds=0.0), + # fixed_frame="odom", + # timeout=rclpy.duration.Duration(seconds=0.1) + # ) + #clk = self.get_clock().now() + #clk.nanoseconds=0 + t = self.tf_buffer.lookup_transform( + #print(self.get_clock().now()) + target_frame=target_frame, + source_frame=ref_frame, + time=rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().error( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + + #print(self.get_clock().now().to_msg().seconds) + print(t.transform.translation.x) + print(t.transform.translation.y) + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.35: # 20 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + pass + #msg.linear.x = 0.25 + #msg.angular.z = scale_rotation * angle + + #self.speed_pub.publish(msg) + + time.sleep(0.1) + rclpy.spin_once(self.tf_listener) + + + + + print("Reached goal") + + self.get_logger().info(f'Finished moving to TF') + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + return + + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'table_frame' + try: + t = self.tf_buffer.lookup_transform( + target_frame, + ref_frame, + time=rclpy.time.Time()) + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance}') + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TfListener() + + rclpy.spin_once(table_detection_node) + + table_detection_node.timer_clb() + + #rclpy.spin(table_detection_node) + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_firmware/robot_firmware/tf2_listener copy.py b/robot_firmware/robot_firmware/tf2_listener copy.py new file mode 100644 index 0000000..eda41bb --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener copy.py @@ -0,0 +1,146 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import math +from statistics import mean +import time + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +class TfListener(Node): + def __init__(self): + super().__init__('tf_listener') + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + #self.timer = self.create_timer(1, self.timer_clb) + + def timer_clb(self): + #self.timer.cancel() + print("timer clb") + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + + scale_forward_speed = 0.1 + scale_rotation = 0.8 + + distance = 99.0 + angle = 0.0 + + msg = Twist() + + #rclpy.spin_once(self.tf_listener) + + while(distance > 0.1): + try: + # t = self.tf_buffer.lookup_transform_full( + # #print(self.get_clock().now()) + # target_frame=target_frame, + # target_time=rclpy.duration.Duration(seconds=0.0), + # source_frame=ref_frame, + # source_time=rclpy.duration.Duration(seconds=0.0), + # fixed_frame="odom", + # timeout=rclpy.duration.Duration(seconds=0.1) + # ) + #clk = self.get_clock().now() + #clk.nanoseconds=0 + t = self.tf_buffer.lookup_transform( + #print(self.get_clock().now()) + target_frame=target_frame, + source_frame=ref_frame, + time=rclpy.time.Time(), + timeout=rclpy.duration.Duration(seconds=1.1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().error( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + + #print(self.get_clock().now().to_msg().seconds) + print(t.transform.translation.x) + print(t.transform.translation.y) + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.35: # 20 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + pass + #msg.linear.x = 0.25 + #msg.angular.z = scale_rotation * angle + + #self.speed_pub.publish(msg) + + time.sleep(0.1) + rclpy.spin_once(self.tf_listener) + + + + + print("Reached goal") + + self.get_logger().info(f'Finished moving to TF') + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + return + + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'table_frame' + try: + t = self.tf_buffer.lookup_transform( + target_frame, + ref_frame, + time=rclpy.time.Time()) + except tf2_ros.TransformException as ex: + self.get_logger().info( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance}') + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TfListener() + + rclpy.spin_once(table_detection_node) + + table_detection_node.timer_clb() + + #rclpy.spin(table_detection_node) + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_firmware/robot_firmware/tf2_listener.py b/robot_firmware/robot_firmware/tf2_listener.py new file mode 100644 index 0000000..e85a2eb --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener.py @@ -0,0 +1,174 @@ +import rclpy +from rclpy.node import Node +from rclpy.duration import Duration +from sensor_msgs.msg import LaserScan +import math +from statistics import mean +import time + +# ros2 run robot_firmware tf2_listener --ros-args -p use_sim_time:=true + +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +from geometry_msgs.msg import PoseStamped, TransformStamped, Twist +from tf_transformations import quaternion_from_euler +import tf2_ros + +class TfListener(Node): + def __init__(self): + super().__init__('tf_listener') + + # Create tf2 listener + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + # Create Twist publisher + self.speed_pub = self.create_publisher(Twist, 'diffbot_base_controller/cmd_vel_unstamped', 3) + + self.timer = self.create_timer(1.0, self.timer_clb, callback_group=MutuallyExclusiveCallbackGroup()) + self.timer1 = self.create_timer(0.1, self.timer_clb1) + + def timer_clb1(self): + pass + + + def timer_clb(self): + self.timer.cancel() + print("timer clb") + + # Try to find transform + target_frame = 'robot_base_link' + ref_frame = 'pre_table_frame' + + # target_frame = 'robot_base_link' + # ref_frame = 'odom' + + scale_forward_speed = 0.1 + scale_rotation = 0.8 + + distance = 99.0 + angle = 0.0 + + msg = Twist() + + #rclpy.spin_once(self.tf_listener) + + while(distance > 0.1): + try: + t = self.tf_buffer.lookup_transform_full( + target_frame=target_frame, + target_time=rclpy.time.Time(), + source_frame=ref_frame, + source_time=rclpy.time.Time(), + fixed_frame='odom', + timeout=rclpy.duration.Duration(seconds=1) + ) + # t = self.tf_buffer.lookup_transform( + # target_frame=target_frame, + # source_frame=ref_frame, + # time=rclpy.time.Time(), # rclpy.time.Duration(seconds=5.0) + # #timeout=rclpy.duration.Duration(seconds=1.1) + # ) + except tf2_ros.TransformException as ex: + self.get_logger().warn( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.20 * angle / abs(angle) + else: + msg.linear.x = 0.1 + msg.angular.z = scale_rotation * 0.20 * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.05) + + + + print("Reached pre goal") + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + distance = 99.0 + ref_frame = 'table_frame' + + while(distance > 0.1): + try: + t = self.tf_buffer.lookup_transform_full( + target_frame=target_frame, + target_time=rclpy.time.Time(), + source_frame=ref_frame, + source_time=rclpy.time.Time(), + fixed_frame='odom', + timeout=rclpy.duration.Duration(seconds=1) + ) + except tf2_ros.TransformException as ex: + self.get_logger().warn( + f'Could not transform {target_frame} to {ref_frame}: {ex}') + time.sleep(0.1) + # TODO + continue + distance = math.sqrt( + t.transform.translation.x ** 2 + + t.transform.translation.y ** 2) + + angle = math.atan2( + t.transform.translation.y, + t.transform.translation.x) + + print(f'Dst: {distance:.2f}, angle: {angle:.2f}') + + if abs(angle) > 0.20: # 13 degree + msg.linear.x = 0.0 + msg.angular.z = 0.10 * angle / abs(angle) + else: + msg.linear.x = 0.15 + msg.angular.z = scale_rotation * 0.20 * angle / abs(angle) + + self.speed_pub.publish(msg) + + time.sleep(0.05) + + print("Reached goal") + + + self.get_logger().info(f'Finished moving to TF') + msg.linear.x = 0.0 + msg.angular.z = 0.0 + self.speed_pub.publish(msg) + self.create_rate(1.0).sleep() + + return + + +def main(args=None): + rclpy.init(args=args) + table_detection_node = TfListener() + + executor = MultiThreadedExecutor(num_threads=4) + executor.add_node(table_detection_node) + + executor.spin() + executor.shutdown() + + table_detection_node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/robot_firmware/robot_firmware/tf2_listener_working.py b/robot_firmware/robot_firmware/tf2_listener_working.py new file mode 100644 index 0000000..7f2cb28 --- /dev/null +++ b/robot_firmware/robot_firmware/tf2_listener_working.py @@ -0,0 +1,42 @@ + +import rclpy +from rclpy.node import Node +import tf2_ros +from geometry_msgs.msg import TransformStamped + +class TransformListener(Node): + def __init__(self): + super().__init__('transform_listener') + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self) + + def get_latest_transform(self, target_frame, source_frame): + try: + # Get the latest transform from the buffer + transform = self.tf_buffer.lookup_transform(target_frame, source_frame, rclpy.time.Time()) + return transform + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + self.get_logger().warn(str(e)) + return None + +def main(args=None): + rclpy.init(args=args) + transform_listener = TransformListener() + + # Specify the target and source frames + target_frame = 'robot_base_link' + source_frame = 'pre_table_frame' + + while rclpy.ok(): + # Get the latest transform + latest_transform = transform_listener.get_latest_transform(target_frame, source_frame) + if latest_transform: + # Print the transform + print(latest_transform.transform.translation) + rclpy.spin_once(transform_listener) + + transform_listener.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/robot_firmware/setup.py b/robot_firmware/setup.py index 1152a11..d81dde2 100644 --- a/robot_firmware/setup.py +++ b/robot_firmware/setup.py @@ -1,4 +1,6 @@ from setuptools import setup +import os +from glob import glob package_name = 'robot_firmware' @@ -10,6 +12,7 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name), glob('launch/*.launch.py')), ], install_requires=['setuptools'], zip_safe=True, @@ -21,7 +24,9 @@ entry_points={ 'console_scripts': [ "main_node = robot_firmware.main_node:main", - "table_detection = robot_firmware.table_detection:main" + "table_detection = robot_firmware.table_detection:main", + "tf2_listener = robot_firmware.tf2_listener:main", + "action = robot_firmware.action:main", ], }, ) diff --git a/robot_firmware/src/table_detection.py b/robot_firmware/src/table_detection.py deleted file mode 100644 index de16d8f..0000000 --- a/robot_firmware/src/table_detection.py +++ /dev/null @@ -1,222 +0,0 @@ - -import rclpy -from rclpy.node import Node -from sensor_msgs.msg import LaserScan -import math -from statistics import mean - -class TableDetectionNode(Node): - def __init__(self): - super().__init__('table_detection_node') - self.laser_scan_subscription = self.create_subscription( - LaserScan, - 'scan', - self.scan_callback, - 5 - ) - self.laser_scan_subscription # prevent unused variable warning - - self.groups = [] - - @staticmethod - def distance_between_polar_coordinates(r1, theta1, r2, theta2): - # Convert polar coordinates to Cartesian coordinates - x1 = r1 * math.cos(theta1) - y1 = r1 * math.sin(theta1) - x2 = r2 * math.cos(theta2) - y2 = r2 * math.sin(theta2) - - # Calculate the distance between the Cartesian coordinates - distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2) - - return distance - - def scan_callback(self, msg): - # Get the range values from the laser scan message - ranges = msg.ranges - - # print(msg) - - # print("#"*10) - # print([f"{i:.2f}" for i in ranges]) - # return - - # Define the minimum and maximum distances for table legs - min_range = 0.2 # Minimum range value for table detection - max_range = 2.5 # Maximum range value for table detection - - max_two_point_dist = 0.03 # Maximum distance between 2 laser reading to be in 1 group - min_group_size = 3 - max_missing_reading = 1 - - table_leg_size = 0.20 - - table_x_size = 0.7 - table_y_size = 0.6 - table_diagnol = math.sqrt(0.7**2 + 0.6**2) - table_size_tolerance = 0.1 - - self.groups.clear() - - group = [] - prev_range = msg.ranges[0] - - scans_too_big = 0 - - # Iterate through the laser scan readings - for i in range(1, len(msg.ranges)): - curr_range = msg.ranges[i] - curr_idx = msg.angle_min + msg.angle_increment * i - - # Check if the current reading is part of the same object - if abs(curr_range - prev_range) < max_two_point_dist: - group.append((curr_range, curr_idx)) - else: - # Add the current group to the list of groups - #if len(group) >= min_group_size: - self.groups.append(group) - - # Start a new group with the current reading - group = [(curr_range, curr_idx)] - - # Update the previous range - prev_range = curr_range - - # Add the last group to the list of groups - if len(group) >= 0: - self.groups.append(group) - - # Print the groups - #print(f"Total {len(self.groups)} groups detected") - - for i, group in enumerate(self.groups): - if len(list(zip(*group))) == 0: - continue - #self.get_logger().info(f'Group {i+1}: {len(group)}, start idx: {group[0][1]*180/3.14:.2f}, end idx: {group[-1][1]*180/3.14:.2f},dist: {mean(list(zip(*group))[0])}') - - # remove too small group - big_groups = [] - for i, group in enumerate(self.groups): - if len(group) >= min_group_size: - big_groups.append(group) - - #print(f"Total {len(big_groups)} groups detected after removing small ones") - - # Merge close groups - merge_groups = [] - for i, group in enumerate(big_groups): - #print(list(zip(*group))) - if i == 0: - merge_groups.append(group) - continue - start_angle = min(list(zip(*group))[1]) - start_dist = list(zip(*group))[0][0] - end_angle = max(list(zip(*merge_groups[-1]))[1]) - end_dist = list(zip(*merge_groups[-1]))[0][-1] - dist_between_start_end = self.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) - - if dist_between_start_end <= max_two_point_dist+0.01: - #print("Append to prev") - - merge_groups[-1].extend(group) - else: - merge_groups.append(group) - - #print(f"Total {len(merge_groups)} groups detected after merging") - - filtered_groups = [] - for i, group in enumerate(merge_groups): - #print(list(zip(*group))) - if len(list(zip(*group))) == 0: - continue - min_dist = min(list(zip(*group))[0]) - max_dist = max(list(zip(*group))[0]) - delta_dist = abs(max_dist - min_dist) - start_angle = min(list(zip(*group))[1]) - start_dist = list(zip(*group))[0][0] - end_angle = max(list(zip(*group))[1]) - end_dist = list(zip(*group))[0][-1] - dist_between_start_end = self.distance_between_polar_coordinates(start_dist, start_angle, end_dist, end_angle) - - if dist_between_start_end <= table_leg_size and \ - len(group) >= min_group_size and \ - delta_dist <= table_leg_size and \ - min_dist >= min_range and \ - max_dist <= max_range: - filtered_groups.append(group) - - #print(f"Total {len(filtered_groups)} groups detected after filtering v0") - - - # for i, group in enumerate(filtered_groups): - # self.get_logger().info(f'Group {i+1}: {len(group)}, start idx: {group[0][1]*180/3.14:.2f}, end idx: {group[-1][1]*180/3.14:.2f},dist: {mean(list(zip(*group))[0])}') - - table = False - # Calculate distacne betwen groups - for i, group in enumerate(filtered_groups): - # Check if there are at least 3 legs left - if not len(filtered_groups) - i >= 3: - #print("NO TABLE") - break - if len(list(zip(*group))) == 0: - continue - current_group_dist = mean(list(zip(*group))[0]) - current_group_angle = mean(list(zip(*group))[1]) - dst = [] - - for j, group_target in enumerate(filtered_groups): - if j <= i: - continue - target_group_dist = mean(list(zip(*group_target))[0]) - target_group_angle = mean(list(zip(*group_target))[1]) - dist_between_start_end = self.distance_between_polar_coordinates(current_group_dist, current_group_angle, target_group_dist, target_group_angle) - dst.append(dist_between_start_end) - - # Analyze distances - #print(dst) - #print([f"{i:.2f}" for i in dst]) - - leg_x = False - leg_y = False - leg_middle = False - for d in dst: - x_size = abs(d - table_x_size) - y_size = abs(d - table_y_size) - diagnol_size = abs(d - table_diagnol) - if x_size < table_size_tolerance and \ - y_size < table_size_tolerance: - if x_size < y_size: - #print("leg_x") - leg_x = True - else: - #print("leg_y") - leg_y = True - elif x_size < table_size_tolerance: - leg_x = True - elif y_size < table_size_tolerance: - leg_y = True - if diagnol_size < table_size_tolerance and \ - (leg_x or leg_y): - #print("leg_mid") - leg_middle = True - - if leg_x and leg_y and leg_middle: - table = True - if table: - self.get_logger().info("TABLE") - else: - self.get_logger().info("NO TABLE") - - - - - -def main(args=None): - rclpy.init(args=args) - table_detection_node = TableDetectionNode() - rclpy.spin(table_detection_node) - table_detection_node.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/robot_firmware_interfaces/CMakeLists.txt b/robot_firmware_interfaces/CMakeLists.txt index ec6e574..d475e80 100644 --- a/robot_firmware_interfaces/CMakeLists.txt +++ b/robot_firmware_interfaces/CMakeLists.txt @@ -14,7 +14,7 @@ find_package(rosidl_default_generators REQUIRED) find_package(action_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "action/Empty.action" + "action/AproachTable.action" ) diff --git a/robot_firmware_interfaces/action/AproachTable.action b/robot_firmware_interfaces/action/AproachTable.action new file mode 100644 index 0000000..c91c22b --- /dev/null +++ b/robot_firmware_interfaces/action/AproachTable.action @@ -0,0 +1,6 @@ +bool dummy_aproach # If true, then only check table existence (for simulation) +--- +int32 result # 0 - success, 1 - table not found, 2 - failed approach +string msg +--- +string state \ No newline at end of file diff --git a/robot_firmware_interfaces/action/Empty.action b/robot_firmware_interfaces/action/Empty.action deleted file mode 100644 index 14d9d49..0000000 --- a/robot_firmware_interfaces/action/Empty.action +++ /dev/null @@ -1,6 +0,0 @@ -#int32 order ---- -bool result -string msg ---- -#int32[] partial_sequence \ No newline at end of file