|
| 1 | +#!/usr/bin/env python3 |
| 2 | +import rclpy |
| 3 | +from rclpy.action import ActionServer, CancelResponse, GoalResponse |
| 4 | +from rclpy.node import Node |
| 5 | +import threading |
| 6 | +import time |
| 7 | + |
| 8 | + |
| 9 | +class MoveToDummy(Node): |
| 10 | + def __init__( |
| 11 | + self, |
| 12 | + name, |
| 13 | + action_class, |
| 14 | + send_feedback_hz=10, |
| 15 | + dummy_plan_time=2.5, |
| 16 | + dummy_motion_time=10.0, |
| 17 | + ): |
| 18 | + """ |
| 19 | + Initialize the MoveToDummy action node. |
| 20 | +
|
| 21 | + Parameters |
| 22 | + ---------- |
| 23 | + name: The name of the action server. |
| 24 | + action_class: The action class to use for the action server. |
| 25 | + send_feedback_hz: The target frequency at which to send feedback. |
| 26 | + dummy_plan_time: How many seconds this dummy node should spend in planning. |
| 27 | + dummy_motion_time: How many seconds this dummy node should spend in motion. |
| 28 | + """ |
| 29 | + super().__init__(name) |
| 30 | + |
| 31 | + self.send_feedback_hz = send_feedback_hz |
| 32 | + self.dummy_plan_time = dummy_plan_time |
| 33 | + self.dummy_motion_time = dummy_motion_time |
| 34 | + self.action_class = action_class |
| 35 | + |
| 36 | + self.active_goal_request = None |
| 37 | + |
| 38 | + self._action_server = ActionServer( |
| 39 | + self, |
| 40 | + action_class, |
| 41 | + name, |
| 42 | + self.execute_callback, |
| 43 | + goal_callback=self.goal_callback, |
| 44 | + cancel_callback=self.cancel_callback, |
| 45 | + ) |
| 46 | + |
| 47 | + def goal_callback(self, goal_request): |
| 48 | + """ |
| 49 | + Accept a goal if this action does not already have an active goal, |
| 50 | + else reject. |
| 51 | +
|
| 52 | + Parameters |
| 53 | + ---------- |
| 54 | + goal_request: The goal request message. |
| 55 | + """ |
| 56 | + self.get_logger().info("Received goal request") |
| 57 | + if self.active_goal_request is None: |
| 58 | + self.get_logger().info("Accepting goal request") |
| 59 | + self.active_goal_request = goal_request |
| 60 | + return GoalResponse.ACCEPT |
| 61 | + self.get_logger().info("Rejecting goal request") |
| 62 | + return GoalResponse.REJECT |
| 63 | + |
| 64 | + def cancel_callback(self, goal_handle): |
| 65 | + """ |
| 66 | + Always accept client requests to cancel the active goal. Note that this |
| 67 | + function should not actually impelement the cancel; that is handled in |
| 68 | + `execute_callback` |
| 69 | +
|
| 70 | + Parameters |
| 71 | + ---------- |
| 72 | + goal_handle: The goal handle. |
| 73 | + """ |
| 74 | + self.get_logger().info("Received cancel request, accepting") |
| 75 | + return CancelResponse.ACCEPT |
| 76 | + |
| 77 | + def plan(self, plan, success): |
| 78 | + """ |
| 79 | + A dummy thread for planning to the target position. This thread |
| 80 | + will sleep for `self.dummy_plan_time` sec and then set the plan to None. |
| 81 | +
|
| 82 | + Parameters |
| 83 | + ---------- |
| 84 | + plan: A mutable object, which will contain the plan once the thread has |
| 85 | + finished. For this dummy thread, it contains None. |
| 86 | + success: A mutable object, which will contain the success status once |
| 87 | + the thread has finished. |
| 88 | + """ |
| 89 | + time.sleep(self.dummy_plan_time) |
| 90 | + plan.append(None) |
| 91 | + success[0] = True |
| 92 | + |
| 93 | + def move(self, plan, success): |
| 94 | + """ |
| 95 | + A dummy thread for moving the robot arm along the plan. This thread |
| 96 | + will sleep for `self.dummy_motion_time` sec and then return success. |
| 97 | +
|
| 98 | + Parameters |
| 99 | + ---------- |
| 100 | + plan: Contains the plan. |
| 101 | + success: A mutable object, which will contain the success status once |
| 102 | + the thread has finished. |
| 103 | + """ |
| 104 | + time.sleep(self.dummy_motion_time) |
| 105 | + success[0] = True |
| 106 | + |
| 107 | + async def execute_callback(self, goal_handle): |
| 108 | + """ |
| 109 | + First, plan to the target position. Then, move to that position. |
| 110 | + As a "dummy node," this specific node will spend `self.dummy_plan_time` |
| 111 | + sec in planning and `self.dummy_motion_time` sec in motion. |
| 112 | +
|
| 113 | + NOTE: In the actual (not dummy) implementation, we will be calling a |
| 114 | + ROS action defined by MoveIt to do both planning and execution. Because |
| 115 | + actions are non-blocking, we won't need to break off separate threads. |
| 116 | + Using separate threads is an artiface of using time.sleep() in this |
| 117 | + dummy implementation. |
| 118 | + """ |
| 119 | + self.get_logger().info("Executing goal...%s" % (goal_handle,)) |
| 120 | + |
| 121 | + # Load the feedback parameters |
| 122 | + feedback_rate = self.create_rate(self.send_feedback_hz) |
| 123 | + feedback_msg = self.action_class.Feedback() |
| 124 | + |
| 125 | + # Start the planning thread |
| 126 | + plan = [] |
| 127 | + plan_success = [False] |
| 128 | + planning_thread = threading.Thread( |
| 129 | + target=self.plan, args=(plan, plan_success), daemon=True |
| 130 | + ) |
| 131 | + planning_thread.start() |
| 132 | + planning_start_time = self.get_clock().now() |
| 133 | + is_planning = True |
| 134 | + |
| 135 | + # Create (but don't yet start) the motion thread |
| 136 | + is_moving = False |
| 137 | + motion_success = [False] |
| 138 | + motion_thread = threading.Thread( |
| 139 | + target=self.move, args=(plan, motion_success), daemon=True |
| 140 | + ) |
| 141 | + |
| 142 | + # Monitor the planning and motion threads, and send feedback |
| 143 | + while rclpy.ok() and (is_planning or is_moving): |
| 144 | + # Check if there is a cancel request |
| 145 | + if goal_handle.is_cancel_requested: |
| 146 | + self.get_logger().info("Goal canceled") |
| 147 | + goal_handle.canceled() |
| 148 | + result = self.action_class.Result() |
| 149 | + result.status = result.STATUS_CANCELED |
| 150 | + self.active_goal_request = None # Clear the active goal |
| 151 | + return result |
| 152 | + |
| 153 | + # Check if the planning thread has finished |
| 154 | + if is_planning: |
| 155 | + if not planning_thread.is_alive(): |
| 156 | + is_planning = False |
| 157 | + if plan_success[0]: # Plan succeeded |
| 158 | + self.get_logger().info( |
| 159 | + "Planning succeeded, proceeding to motion" |
| 160 | + ) |
| 161 | + # Start the motion thread |
| 162 | + motion_thread.start() |
| 163 | + motion_start_time = self.get_clock().now() |
| 164 | + is_moving = True |
| 165 | + continue |
| 166 | + else: # Plan failed |
| 167 | + self.get_logger().info("Planning failed, aborting") |
| 168 | + # Abort the goal |
| 169 | + goal_handle.abort() |
| 170 | + result = self.action_class.Result() |
| 171 | + result.status = result.STATUS_PLANNING_FAILED |
| 172 | + self.active_goal_request = None # Clear the active goal |
| 173 | + return result |
| 174 | + |
| 175 | + # Check if the motion thread has finished |
| 176 | + if is_moving: |
| 177 | + if not motion_thread.is_alive(): |
| 178 | + is_moving = False |
| 179 | + if motion_success[0]: |
| 180 | + self.get_logger().info("Motion succeeded, returning") |
| 181 | + # Succeed the goal |
| 182 | + goal_handle.succeed() |
| 183 | + result = self.action_class.Result() |
| 184 | + result.status = result.STATUS_SUCCESS |
| 185 | + self.active_goal_request = None # Clear the active goal |
| 186 | + return result |
| 187 | + else: |
| 188 | + self.get_logger().info("Motion failed, aborting") |
| 189 | + # Abort the goal |
| 190 | + goal_handle.abort() |
| 191 | + result = self.action_class.Result() |
| 192 | + result.status = result.STATUS_MOTION_FAILED |
| 193 | + self.active_goal_request = None # Clear the active goal |
| 194 | + return result |
| 195 | + |
| 196 | + # Send feedback |
| 197 | + feedback_msg.is_planning = is_planning |
| 198 | + if is_planning: |
| 199 | + feedback_msg.planning_time = ( |
| 200 | + self.get_clock().now() - planning_start_time |
| 201 | + ).to_msg() |
| 202 | + elif is_moving: |
| 203 | + # TODO: In the actual (not dummy) implementation, this should |
| 204 | + # return the distance (not time) the robot has yet to move. |
| 205 | + feedback_msg.motion_initial_distance = self.dummy_motion_time |
| 206 | + elapsed_time = self.get_clock().now() - motion_start_time |
| 207 | + elapsed_time_float = elapsed_time.nanoseconds / 1.0e9 |
| 208 | + feedback_msg.motion_curr_distance = ( |
| 209 | + self.dummy_motion_time - elapsed_time_float |
| 210 | + ) |
| 211 | + self.get_logger().info("Feedback: %s" % feedback_msg) |
| 212 | + goal_handle.publish_feedback(feedback_msg) |
| 213 | + |
| 214 | + # Sleep for the specified feedback rate |
| 215 | + feedback_rate.sleep() |
| 216 | + |
| 217 | + # If we get here, something went wrong |
| 218 | + self.get_logger().info("Unknown error, aborting") |
| 219 | + goal_handle.abort() |
| 220 | + result = self.action_class.Result() |
| 221 | + result.status = result.STATUS_UNKNOWN |
| 222 | + self.active_goal_request = None # Clear the active goal |
| 223 | + return result |
0 commit comments