def cb_srv_clear(self, request):
self.totem_circle_command = []
print("Clear")
return TriggerResponse()
def handle_choreograph_play_next(self, req):
resp = self.spot_wrapper._play_next_choreograph()
return TriggerResponse(resp[0], resp[1])
def handle_claim(self, req):
"""ROS service handler for the claim service"""
resp = self.spot_wrapper.claim()
return TriggerResponse(resp[0], resp[1])
def enable_cb(self, req):
self.enabled = True
self.subscribe()
return TriggerResponse(True, "auto recover enabled")
def left_cb(req):
rospy.loginfo("recovering gripper_left")
os.system("rosnode kill /gripper_left/gripper_left_node")
return TriggerResponse()
def handle_safe_power_off(self, req):
"""ROS service handler for the safe-power-off service"""
resp = self.spot_wrapper.safe_power_off()
return TriggerResponse(resp[0], resp[1])
def handle_estop_soft(self, req):
"""ROS service handler to soft-eStop the robot. The robot will try to settle on the ground before cutting power to the motors"""
resp = self.spot_wrapper.assertEStop(False)
return TriggerResponse(resp[0], resp[1])
def as_trigger_callback(self, *args, **kwargs):
result = func(self)
return TriggerResponse(result, execution_state_msg[result])
def parseTask(self,task):
response = TriggerResponse()
# IF THIS IS A PLAN THAT REQUIRES SUPERVISION
if self._aware:
# if the task isn't the starting task.
if task.action!='START':
# if the energy cost mu or std = 0, then this task is buggy.
if task.cost_mu==0 or task.cost_std ==0:
rospy.logwarn("Buggy task, skipping. {}".format(task))
self.plan.skipTask()
return self.parseTask(self.plan.getTask())
# GET WAYPOINT COMPONENTS READY TO SEND TO THE GUIDANCE NODE
geo_msg = GeoPoseStamped()
geo_msg.header.frame_id="utm"
geo_msg.header.stamp = rospy.Time.now()
self.checkValidTask(task)
# Look at the action property and decide on what to do from there:
if task.action == "WP":
# waypoint task, configure for AP mode to a GPS waypoint.
if self.changeMode("/tau_com/AP"):
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
# turn off the timing lock
self._timing_lock = False
response.success = True
response.message = "Waypoint Task Selected, executing"
return response
else:
response.success = False
response.message = "Mode not configured to AP."
return response
elif task.action == "HP":
# hold position task, configure DP mode at a GPS waypoint and orientation for a set time.
if self.changeMode("/tau_com/DP"):
geo_msg.pose.position.altitude=0
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(*tf.quaternion_from_euler(0,0,task.data[2]))
self.hptime = task.data[3]
self.hptask = True
self.task_pub.publish(geo_msg)
# turn off the timing lock
self._timing_lock = False
response.success = True
response.message ="Hold Pose Task Selected, executing."
return response
else:
response.success = False
response.message = "Mode not configured to DP."
return response
elif task.action == "ROOT":
# root task reached, notify the operator that the mission is complete.
if self.changeMode("__none"):
self.mode="idle"
self._timing_lock = False
self.status_pub.publish("ASV currently in mode: {}".format(self.mode))
rospy.loginfo("Final task completed, now idling.")
response.success = True
response.message = "Final task completed, now idling."
return response
else:
rospy.logerr("Couldn't disable output topic, but mission complete.")
response.success = False
response.message = "Couldn't Idle for some reason?"
return response
elif task.action == "START":
# start task, is a waypoint task to move the vehicle to the starting position.
if self.changeMode("/tau_com/AP"):
self._start_flag = True
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
self._timing_lock = False
rospy.loginfo("Moving to starting waypoint of plan.")
response.success = True
response.message = "AP mode to starting point."
return response
else:
rospy.logerr("Couldn't configure to AP mode.")
response.success = False
response.message = "Couldn't configure to AP mode."
return response
elif task.action == "HOME":
# home task, is a waypoint task to move the vehicle to the starting position.
if self.changeMode("/tau_com/AP"):
geo_msg.pose.position.altitude=-1
geo_msg.pose.position.latitude=task.data[0]
geo_msg.pose.position.longitude=task.data[1]
geo_msg.pose.orientation = Quaternion(0,0,0,1)
self.task_pub.publish(geo_msg)
self._timing_lock = False
rospy.loginfo("Moving to rendezvous point.")
response.success = True
response.message = "AP mode to home point."
return response
else:
rospy.logerr("Couldn't configure to AP mode.")
response.success = False
response.message = "Couldn't configure to AP mode."
return response
else:
rospy.logerr("Task of type {} is not supported, getting next task.".format(task.action))
self.plan.skipTask()
return self.parseTask(self.plan.getTask())
def pick_aruco(self, string_operation):
#rospy.sleep(2.0)
if string_operation == "pick":
self.prepare_robot()
# Detect object
rospy.loginfo(
"spherical_grasp_gui: Waiting for an aruco detection")
aruco_pose = rospy.wait_for_message('/aruco_single/pose',
PoseStamped)
aruco_pose.header.frame_id = self.strip_leading_slash(
aruco_pose.header.frame_id)
rospy.loginfo("Got: " + str(aruco_pose))
rospy.loginfo("spherical_grasp_gui: Transforming from frame: " +
aruco_pose.header.frame_id + " to 'base_footprint'")
ps = PoseStamped()
ps.pose.position = aruco_pose.pose.position
ps.pose.orientation = aruco_pose.pose.orientation
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
ps.header.frame_id = aruco_pose.header.frame_id
transform_ok = False
while not transform_ok and not rospy.is_shutdown():
try:
transform = self.tfBuffer.lookup_transform(
"base_footprint", ps.header.frame_id, rospy.Time(0))
aruco_ps = do_transform_pose(ps, transform)
transform_ok = True
except tf2_ros.ExtrapolationException as e:
rospy.logwarn(
"Exception on transforming point... trying again \n(" +
str(e) + ")")
rospy.sleep(0.01)
ps.header.stamp = self.tfBuffer.get_latest_common_time(
"base_footprint", aruco_pose.header.frame_id)
pick_g = PickUpPoseGoal()
rospy.loginfo("Setting cube pose based on ArUco detection")
pick_g.object_pose.pose.position = aruco_ps.pose.position
pick_g.object_pose.pose.position.z -= 0.1 * (1.0 / 2.0)
rospy.loginfo("aruco pose in base_footprint:" + str(pick_g))
pick_g.object_pose.header.frame_id = 'base_footprint'
pick_g.object_pose.pose.orientation.w = 1.0
self.detected_pose_pub.publish(pick_g.object_pose)
rospy.loginfo("Gonna pick:" + str(pick_g))
self.pick_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# rospy.loginfo("Moving arm to a safe pose")
# pmg = PlayMotionGoal()
# pmg.motion_name = 'pick_final_pose'
# pmg.skip_planning = False
# self.play_m_as.send_goal_and_wait(pmg)
# rospy.loginfo("Raise object done.")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.pick_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to pick, not trying further")
return TriggerResponse(False, "Failed to pick")
self.pick_g = pick_g
return TriggerResponse(True, "Succeeded")
elif string_operation == "place":
if not self.pick_g:
rospy.logerr("Failed to place, no object position known")
return TriggerResponse(
False, "Failed to place, no object position known")
pick_g = self.pick_g
# Move torso to its maximum height
self.lift_torso()
# Raise arm
rospy.loginfo("Moving arm to a safe pose")
pmg = PlayMotionGoal()
pmg.motion_name = 'prepare_grasp' #'pick_final_pose'
pmg.skip_planning = False
self.play_m_as.send_goal_and_wait(pmg)
rospy.loginfo("Raise object done.")
# Place the object back to its position
rospy.loginfo("Gonna place near where it was")
pick_g.object_pose.pose.position.z += 0.05
pick_g.object_pose.pose.position.y -= 0.35
self.place_as.send_goal_and_wait(pick_g)
rospy.loginfo("Done!")
# Move to safe
home = False
i = 0
#self.move_back(5)
while not home and i < 3:
home = self.move_arm_home()
i += 1
rospy.sleep(0.5)
result = self.place_as.get_result()
if str(moveit_error_dict[result.error_code]) != "SUCCESS":
rospy.logerr("Failed to place")
return TriggerResponse(False, "Failed to place")
self.pick_g = None
return TriggerResponse(True, "Succeeded")
def trigger_cb(req):
return TriggerResponse()
def markers_off(self, req):
self.set_markers([PIXELS_BLACK])
return TriggerResponse()
def markers_on(self, req):
self.set_markers([PIXELS_SPECTRUM])
return TriggerResponse()
def cb_srv_start(self, request):
print("Start")
self.start = True
return TriggerResponse()
def handle_stand(self, req):
"""ROS service handler for the stand service"""
resp = self.spot_wrapper.stand()
return TriggerResponse(resp[0], resp[1])
def stop(self, req=None):
self.tracer.stop()
return TriggerResponse(success=True)
def handle_power_on(self, req):
"""ROS service handler for the power-on service"""
resp = self.spot_wrapper.power_on()
return TriggerResponse(resp[0], resp[1])
def reload_properties_callback(self, request):
self.load_properties()
return TriggerResponse(success=True)
def handle_estop_hard(self, req):
"""ROS service handler to hard-eStop the robot. The robot will immediately cut power to the motors"""
resp = self.spot_wrapper.assertEStop(True)
return TriggerResponse(resp[0], resp[1])
def vi_srv_cb(self, request):
rospy.Timer(rospy.Duration(1), self.timer_cb, oneshot=True)
return TriggerResponse(success=True)
def handle_estop_disengage(self, req):
"""ROS service handler to disengage the eStop on the robot."""
resp = self.spot_wrapper.disengageEStop()
return TriggerResponse(resp[0], resp[1])
def get_logging_callback(self, req):
# Return current logging status
return TriggerResponse(success = self.start_logging)
def disable_cb(self, req):
self.enabled = False
self.unsubscribe()
return TriggerResponse(True, "auto recover disabled")
def trigger_response(request):
return TriggerResponse(
success = True
message = "Successful"
)
def onoff_response(self, onoff):
d = TriggerResponse()
d.success = self.set_power(onoff)
d.message = "ON" if self.is_on else "OFF"
return d
def service_topic_name_cb(self, request):
# Because this is a service call back and not a message callback, it takes in a Request, and not a Message.
# This callback must also return a Response. Returning a response could look like this:
return TriggerResponse(True, "error_message_goes_here")
def handle_battery_pose(self, req):
resp = self.spot_wrapper.battery_change_pose()
return TriggerResponse(resp[0], resp[1])
def handle_self_right(self, req):
"""ROS service handler for the self-right service"""
resp = self.spot_wrapper.self_right()
return TriggerResponse(resp[0], resp[1])
def handle_release(self, req):
"""ROS service handler for the release service"""
resp = self.spot_wrapper.release()
return TriggerResponse(resp[0], resp[1])
def reset_callback(self, data):
self.status = "Standby"
self.stop_actions()
self.manual_used = False
self.loaded = False
return TriggerResponse(success=True)
