def _step_complete(self, goal): if goal is not None: with self._goal_handle_lock: self.publish_process_state() res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" goal.set_succeeded(res) if (self._goal_handle == goal): self._goal_handle = None
def _step_failed(self, err, goal): if goal is None: raise err else: with self._goal_handle_lock: self.publish_process_state() res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" res.error_msg = str(err) goal.set_aborted(result=res) if (self._goal_handle == goal): self._goal_handle = None
def done_cb(err): rospy.loginfo("safe_kinematic_controller generated: %s", str(err)) if (goal is not None): if err is None: self._step_complete(goal) else: with self._goal_handle_lock: if self._goal_handle == goal: self._goal_handle = None res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" res.error_msg = str(err) goal.set_aborted(result=res) rospy.loginfo("safe_kinematic_controller generated: %s", str(err))
def done_cb(status, result): rospy.loginfo("ibvs placement generated success") if (goal is not None): if status == actionlib.GoalStatus.SUCCEEDED: self._step_complete(goal) else: with self._goal_handle_lock: if self._goal_handle == goal: self._goal_handle = None res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" res.error_msg = str(result.error_msg) goal.set_aborted(result=res) rospy.loginfo("pbvs placement generated: %s", result.error_msg)
def _begin_step(self, goal): if goal is not None: with self._goal_handle_lock: if self._goal_handle is not None: res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" res.error_msg = "Attempt to execute new step while previous step running" goal.set_rejected(result=res) rospy.loginfo( "Attempt to execute new step while previous step running" ) raise Exception( "Attempt to execute new step while previous step running" ) else: goal.set_accepted() self._goal_handle = goal
def _execute_path(self, path, goal, ft_stop=False): if goal is None: self.controller_commander.execute_trajectory(path, ft_stop=ft_stop) return def done_cb(err): rospy.loginfo("safe_kinematic_controller generated: %s", str(err)) if (goal is not None): if err is None: self._step_complete(goal) else: with self._goal_handle_lock: if self._goal_handle == goal: self._goal_handle = None res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" res.error_msg = str(err) goal.set_aborted(result=res) rospy.loginfo("safe_kinematic_controller generated: %s", str(err)) with self._goal_handle_lock: if goal.get_goal_status().status != actionlib.GoalStatus.ACTIVE: if self._goal_handle == goal: self._goal_handle = None res = ProcessStepResult() res.state = self.state res.target = self.current_target if self.current_target is not None else "" res.payload = self.current_payload if self.current_payload is not None else "" res.error_msg = str(err) goal.set_aborted(result=res) rospy.loginfo("goal aborted before move") return self.controller_commander.async_execute_trajectory(path, done_cb=done_cb, ft_stop=ft_stop)
def execute_cb(self, goal): command = goal.command if command == "plan_pickup_prepare": self.controller.plan_pickup_prepare(goal.target) elif command == "stop": self.controller.stop() elif command == "move_pickup_prepare": self.controller.move_pickup_prepare() elif command == "plan_to_reset_position": self.controller.plan_to_reset_position() elif command == "plan_pickup_lower": self.controller.plan_pickup_lower() elif command == "move_pickup_lower": self.controller.move_pickup_lower() elif command == "plan_pickup_grab_first_step": self.controller.plan_pickup_grab_first_step() elif command == "move_pickup_grab_first_step": self.controller.move_pickup_grab_first_step() elif command == "plan_pickup_grab_second_step": self.controller.plan_pickup_grab_second_step() elif command == "move_pickup_grab_second_step": self.controller.move_pickup_grab_second_step() elif command == "plan_pickup_raise": self.controller.plan_pickup_raise() elif command == "move_pickup_raise": self.controller.move_pickup_raise() elif command == "plan_transport_payload": self.controller.plan_transport_payload(goal.target) elif command == "move_transport_payload": self.controller.move_transport_payload() elif command == "plan_place_lower": self.controller.plan_place_lower() elif command == "move_place_lower": self.controller.move_place_lower() elif command == "plan_place_set_first_step": self.controller.plan_place_set_first_step() elif command == "move_place_set_first_step": self.controller.move_place_set_first_step() elif command == "plan_place_set_second_step": self.controller.plan_place_set_second_step() elif command == "move_place_set_second_step": self.controller.move_place_set_second_step() elif command == "plan_place_raise": self.controller.plan_place_raise() elif command == "move_place_raise": self.controller.move_place_raise() elif command == "move": self.controller.move(goal.target) elif command == "move_with_force_stop": self.controller.move_with_force_stop(goal.target) else: assert False, "Invalid command" res = ProcessStepResult() res.state = self.controller.state res.target = self.controller.current_target if self.controller.current_target is not None else "" res.payload = self.controller.current_payload if self.controller.current_payload is not None else "" self.server.set_succeeded(res)