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)