def __init__(self): print("Getting robot state... ") self.rs = RobotEnable(CHECK_VERSION) init_state = self.rs.state().enabled self.rs.enable() self.pose = PoseStamped() self.prevX = 0.65 self.prevY = 0.0 self.waypoints = [] ap = argparse.ArgumentParser() ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough") ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)") ap.add_argument("-b", "--box", type=bool, help="Test box edges at progam startup (true/false)") ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position") self.args = vars(ap.parse_args()) if self.args["condition"] is None: self.args["condition"] = "calm" if not self.args["randomize"]: self.args["randomize"] = False if not self.args["box"]: self.args["box"] = False outargs = dict(zip(self.args.keys(), self.args.values())) print('') print('') print(outargs) print('') print('--------------------------------------------------------------------------------------------') print('')
def __init__(self): self._done = False self._head = Head() self._robot_state = RobotEnable(CHECK_VERSION) self._init_state = self._robot_state.state().enabled rospy.Subscriber("robot/head/head_state", HeadState, self.straighten)
def __init__(self): """ 'Wobbles' both arms by commanding joint velocities sinusoidally. """ print("Initializing node... ") rospy.init_node("rsdk_dish_rack") rospy.on_shutdown(self.clean_shutdown) self._right_arm = limb.Limb("right") self._right_joint_names = self._right_arm.joint_names() # control parameters self._rate = 20.0 # Hz self._missed_cmds = 3 self.control_rate = rospy.Rate(self._rate) print("Getting robot state... ") self._rs = RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self._init_joint_angles = [ self._right_arm.joint_angle(joint_name) for joint_name in self._right_joint_names ] rospy.set_param('named_poses/right/poses/neutral', self._init_joint_angles) self._right_arm.set_command_timeout( (1.0 / self._rate) * self._missed_cmds)
def __init__(self): self._done = False self._head = Head() self._limb=Limb() self._robot_state = RobotEnable(CHECK_VERSION) self._init_state = self._robot_state.state().enabled self._robot_state.enable() self._display = HeadDisplay() face_forward_service = rospy.Service('head/face_forward', FaceForward, self.face_forward) rotate_head_service= rospy.Service('head/pan_to_angle', PanToAngle, self.pan_to_angle)
def __init__(self): print("Getting robot state... ") if ACCESS_ROBOT: self.rs = RobotEnable(CHECK_VERSION) self.rs.enable() self.STOP = False self.pose = PoseStamped() self.prevX = 0.65 self.prevY = 0.0 self.l = Lights() self.l_name = 'head_green_light' self.r_name = 'head_red_light' ap = argparse.ArgumentParser() ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough") ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)") ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup (true/false)") ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position") self.args = vars(ap.parse_args()) if self.args["condition"] is None: self.args["condition"] = "calm" if not self.args["randomize"]: self.args["randomize"] = False if not self.args["box"]: self.args["box"] = False outargs = dict(zip(self.args.keys(), self.args.values())) print('') print('') print(outargs) print('') print('') print('') print('') self.parser = InputData() # ENABLE RED LIGHT WHILE WAITING FOR USER INPUT self.l.set_light_state(self.r_name, on=True) try: input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ") except Exception: print("") # ENABLE GREEN LIGHT WHEN PROGRAM IS RUNNING self.l.set_light_state(self.r_name, on=False) self.l.set_light_state(self.l_name, on=True)
def __init__(self): print("Getting robot state... ") if ACCESS_ROBOT: self.rs = RobotEnable(CHECK_VERSION) self.rs.enable() self.STOP = False self.pose = PoseStamped() self.prevX = 0.65 self.prevY = 0.0 ap = argparse.ArgumentParser() ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough") ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition") ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup") ap.add_argument("-l", "--line", action='store_true', help="Move in straight line to test Doppler effect") ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position") self.args = vars(ap.parse_args()) if self.args["condition"] is None: self.args["condition"] = "calm" if not self.args["randomize"]: self.args["randomize"] = False if not self.args["box"]: self.args["box"] = False if not self.args["line"]: self.args["line"] = False outargs = dict(zip(self.args.keys(), self.args.values())) print('') print('') print(outargs) print('') print('') print('') print('') self.parser = InputData() try: input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ") except Exception: print("")
class Sawyer_Head: def __init__(self): self._done = False self._head = Head() self._robot_state = RobotEnable(CHECK_VERSION) self._init_state = self._robot_state.state().enabled rospy.Subscriber("robot/head/head_state", HeadState, self.straighten) def straighten(self,headState): try: self._head.set_pan(math.pi/2,speed=1.0,timeout=1.0,active_cancellation=True) except OSError, e: pass
class PositionControl: def __init__(self): print("Getting robot state... ") self.rs = RobotEnable(CHECK_VERSION) init_state = self.rs.state().enabled self.rs.enable() self.pose = PoseStamped() self.prevX = 0.65 self.prevY = 0.0 self.waypoints = [] ap = argparse.ArgumentParser() ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough") ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)") ap.add_argument("-b", "--box", type=bool, help="Test box edges at progam startup (true/false)") ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position") self.args = vars(ap.parse_args()) if self.args["condition"] is None: self.args["condition"] = "calm" if not self.args["randomize"]: self.args["randomize"] = False if not self.args["box"]: self.args["box"] = False outargs = dict(zip(self.args.keys(), self.args.values())) print('') print('') print(outargs) print('') print('--------------------------------------------------------------------------------------------') print('') def gen_path(self): limb = Limb() for j in range (0, 1): if self.args["randomize"]: aX = random.uniform(0.0,0.17) aY = random.uniform(0.0,0.17) aZ = random.uniform(0.0,0.17) bX = 0#random.uniform(0.01,0.011) bY = 0#random.uniform(0.01,0.011) bZ = 0#random.uniform(0.01,0.011) gX = random.randint(0,1) gY = random.randint(0,1) gZ = random.randint(0,1) damper = 35 print(aX, aY, aZ, bX, bY, bZ, gX, gY, gZ, damper) else: if self.args["condition"] == "rough": aX = 0.1120 aY = 0.1230 aZ = 0.1568 bX = 0.0#0.0107 bY = 0.0#0.0108 bZ = 0.0#0.0120 self. gX = 0 gY = 0 gZ = 0 damper = 20
class Sawyer_Head: def get_neutral_base_joint_angle(self): angle=rospy.get_param("named_poses/right/poses/shipping")[1] return angle def __init__(self): self._done = False self._head = Head() self._limb=Limb() self._robot_state = RobotEnable(CHECK_VERSION) self._init_state = self._robot_state.state().enabled self._robot_state.enable() self._display = HeadDisplay() face_forward_service = rospy.Service('head/face_forward', FaceForward, self.face_forward) rotate_head_service= rospy.Service('head/pan_to_angle', PanToAngle, self.pan_to_angle) # arrow_key_control_servie = rospy.Service('head/arrow_key_control', PanToAngle, self.turn_with_arrow_keys) # rospy.Subscriber("robot/head/head_state", HeadState, self.straighten) # def straighten(self,headState): # try: # self._head.set_pan(math.pi/2,speed=1.0,timeout=1.0,active_cancellation=True) # except OSError, e: # print(e) def get_base_joint_angle(self): base_joint_name=self._limb.joint_names()[0] return self._limb.joint_angle(base_joint_name) def set_angle(self,angle): ''' Sets head to the desired angle 0 - left pi/2 - forward pi - right ''' self._head.set_pan(angle) def pan_to_angle(self, req): try: angle=req.theta self._head.set_pan(angle) return PanToAngleResponse(True) except: return PanToAngleResponse(False) def face_forward(self, object): ''' Sets head facing forward regardless of body position ''' try: base_angle= self.get_base_joint_angle() head_angle= self._head.pan() angle=-1*base_angle+ math.pi/2 self.set_angle(angle) return FaceForwardResponse(True) except: return FaceForwardResponse(False) def turn_head(self, req): try: head_angle=self._head.pan() if req.left: self.set_angle(head_angle+math.radians(3)) else: self.set_angle(head_angle-math.radians(3)) except: rospy.logwarn("can't turn head any further") def display(self, face): ''' display a face on sawyer ''' pass
class RealEnv(Env): timestep = 0 ep_len = 128 def step(self, action): cmd = dict([(joint, action) for joint, action in zip(self._right_joint_names, action)]) self._right_arm.set_joint_velocities(cmd) self.control_rate.sleep() self.timestep += 1 ob = self._get_obs() done = (self.timestep == self.ep_len) return ob, 0, done, dict() def _get_obs(self): return [ self._right_arm.joint_angle(joint_name) for joint_name in self._right_joint_names ] def reset(self): stop_action = [0.] * len(self._right_joint_names) cmd = dict([ (joint, action) for joint, action in zip(self._right_joint_names, stop_action) ]) self._right_arm.set_joint_velocities(cmd) self.timestep = 0 input("Move the arm into open space and press Enter to continue.") # SETS TO A NEUTRAL POSITION. Remove from dish rack first. self.set_neutral() return self._get_obs() def render(self, mode='human'): pass def __init__(self): """ 'Wobbles' both arms by commanding joint velocities sinusoidally. """ print("Initializing node... ") rospy.init_node("rsdk_dish_rack") rospy.on_shutdown(self.clean_shutdown) self._right_arm = limb.Limb("right") self._right_joint_names = self._right_arm.joint_names() # control parameters self._rate = 20.0 # Hz self._missed_cmds = 3 self.control_rate = rospy.Rate(self._rate) print("Getting robot state... ") self._rs = RobotEnable(CHECK_VERSION) self._init_state = self._rs.state().enabled print("Enabling robot... ") self._rs.enable() self._init_joint_angles = [ self._right_arm.joint_angle(joint_name) for joint_name in self._right_joint_names ] rospy.set_param('named_poses/right/poses/neutral', self._init_joint_angles) self._right_arm.set_command_timeout( (1.0 / self._rate) * self._missed_cmds) def set_neutral(self): """ Sets both arms back into a neutral pose. """ print("Moving to neutral pose...") self._right_arm.move_to_neutral() def clean_shutdown(self): """ Switches out of joint torque mode to exit cleanly """ print("\nExiting example...") self._right_arm.exit_control_mode() if not self._init_state and self._rs.state().enabled: print("Disabling robot...") self._rs.disable() def make_cmd(self, joint_names, action): return dict([(joint, action) for joint, action in zip(joint_names, action)])
class Drone: def __init__(self): print("Getting robot state... ") if ACCESS_ROBOT: self.rs = RobotEnable(CHECK_VERSION) self.rs.enable() self.STOP = False self.pose = PoseStamped() self.prevX = 0.65 self.prevY = 0.0 ap = argparse.ArgumentParser() ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough") ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition") ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup") ap.add_argument("-l", "--line", action='store_true', help="Move in straight line to test Doppler effect") ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position") self.args = vars(ap.parse_args()) if self.args["condition"] is None: self.args["condition"] = "calm" if not self.args["randomize"]: self.args["randomize"] = False if not self.args["box"]: self.args["box"] = False if not self.args["line"]: self.args["line"] = False outargs = dict(zip(self.args.keys(), self.args.values())) print('') print('') print(outargs) print('') print('') print('') print('') self.parser = InputData() try: input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ") except Exception: print("") # ENSURE THE WAYPOINTS LIST IS CLEARED AT TERMINATION OF PROGRAM def clean_shutdown(self): print("Stopping arm...") try: self.STOP = True self.move(point_list = None) except: print("There may have been an error exiting") print("Stop successful, exiting...") return def sim_drone(self): M = self.parser.inputMatrix() point_list = list() i = 0 for waypoint in M: #point = [ waypoint[2][0],-waypoint[2][1],-waypoint[2][2],waypoint[3][0],waypoint[3][1],waypoint[3][2] ] point = [ waypoint[2][0],-waypoint[2][1],-waypoint[2][2],0,0,0 ] point_list.append(point) print(point) if i > 150: # Each point represent 0.1 seconds of flight time in simulator break else: i += 1 self.move(wait=True, point_list=point_list, MAX_LIN_ACCL=2.5) # CONTAINS WAYPOINTS TO TRACE BOX AT START OF PROGRAM def trace_box(self): print("I am tracing a box") point_list = list() point = [0.0, 0.25, 0.0, 0.0, 0.0, 0.0] point_list.append(point) point = [0.0, 0.25, 0.1, 0.0, 0.0, 0.0] point_list.append(point) point = [0.0, -0.25, 0.1, 0.0, 0.0, 0.0] point_list.append(point) point = [0.0, -0.25, -0.1, 0.0, 0.0, 0.0] point_list.append(point) point = [0.0, 0.25, -0.1, 0.0, 0.0, 0.0] point_list.append(point) point = [0.0, 0.25, 0.0, 0.0, 0.0, 0.0] point_list.append(point) success = self.move(wait=True, point_list=point_list, MAX_LIN_SPD=0.5, MAX_LIN_ACCL=0.75) return success def follow_line(self): print("I am moving in a line") point_list = list() point = [0.0, 0.4, 0.0, 0.0, 0.0, 0.0] point_list.append(point) point = [0.0, -0.4, 0.0, 0.0, 0.0, 0.0] point_list.append(point) success = self.move(wait=True, point_list=point_list, MAX_LIN_SPD=1.0, MAX_LIN_ACCL=20.0) return success def moveToNeutral(self): print("\n --- Returning to neutral position (0.65, 0.0, 0.4, 0.0, 0.0, 0.0 ---") point = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] point_list = [point] success = self.move(wait=True, point_list=point_list, MAX_LIN_SPD=0.5, MAX_LIN_ACCL=0.75) return success # NEED TO TO SWITCH JOINT CONTROL MODE TO JOINT AND USE AN IK SOLVER TO INTERPOLATE JOINT POSITIONS # FUNCTION TO ABSTRACT CONTORL OF ARM def move(self, point_list, wait = True, MAX_LIN_SPD=7.0, MAX_LIN_ACCL=1.5): # one point = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg] try: limb = Limb() # point_list = [pointA, pointB, pointC, ...] traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.CARTESIAN traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) except: print("There may have been an error while exiting") if self.STOP: traj.stop_trajectory() return True wpt_opts = MotionWaypointOptions(max_linear_speed=MAX_LIN_SPD, max_linear_accel=MAX_LIN_ACCL, corner_distance=0.002) for point in point_list: q_base = quaternion_from_euler(0, 0, math.pi/2) #q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5])) q_rot = quaternion_from_euler(point[3], point[4], point[5]) q = quaternion_multiply(q_rot, q_base) newPose = PoseStamped() newPose.header = Header(stamp=rospy.Time.now(), frame_id='base') newPose.pose.position.x = point[0] + 0.65 newPose.pose.position.y = point[1] + 0.0 newPose.pose.position.z = point[2] + 0.4 newPose.pose.orientation.x = q[0] newPose.pose.orientation.y = q[1] newPose.pose.orientation.z = q[2] newPose.pose.orientation.w = q[3] waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) waypoint.set_cartesian_pose(newPose, "right_hand", limb.joint_ordered_angles()) traj.append_waypoint(waypoint.to_msg()) if(wait): print(" \n --- Sending trajectory and waiting for finish --- \n") result = traj.send_trajectory(wait_for_result=wait) if result is None: rospy.logerr('Trajectory FAILED to send') success = False elif result.result: rospy.loginfo('Motion controller successfully finished the trajcetory') success = True else: rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId) success = False else: print("\n --- Sending trajector w/out waiting --- \n") traj.send_trajectory(wait_for_result=wait) success = True return success # MAIN CONTROL LOOP def fly(self, weather="calm"): print("Flying") self.moveToNeutral() if self.args["box"]: self.trace_box() self.moveToNeutral() if self.args["line"]: self.follow_line() self.moveToNeutral() return #self.sim_drone() self.move(wait=True, point_list=[[-0.011,0.046,0.015,-0.53,0.023,-6.5e06],[-0.012, 0.04, 0.014, -0.53, 0.022, -6.2e-06], [0.0, 0.0, 0.0, 0, 0, 0]]) #rate = rospy.Rate(RATE) #while not rospy.is_shutdown(): # rate.sleep() return
class Drone: def __init__(self): print("Getting robot state... ") if ACCESS_ROBOT: self.rs = RobotEnable(CHECK_VERSION) self.rs.enable() self.STOP = False self.pose = PoseStamped() self.prevX = 0.65 self.prevY = 0.0 self.l = Lights() self.l_name = 'head_green_light' self.r_name = 'head_red_light' ap = argparse.ArgumentParser() ap.add_argument("-c", "--condition", type=str, help="Condition of the environment: calm, average, rough") ap.add_argument("-r", "--randomize", action='store_true', help="Randomly generate trajectory, overrides condition (true/false)") ap.add_argument("-b", "--box", action='store_true', help="Test box edges at progam startup (true/false)") ap.add_argument("--reset", action='store_true', help="Reset the arm to it's beginning position") self.args = vars(ap.parse_args()) if self.args["condition"] is None: self.args["condition"] = "calm" if not self.args["randomize"]: self.args["randomize"] = False if not self.args["box"]: self.args["box"] = False outargs = dict(zip(self.args.keys(), self.args.values())) print('') print('') print(outargs) print('') print('') print('') print('') self.parser = InputData() # ENABLE RED LIGHT WHILE WAITING FOR USER INPUT self.l.set_light_state(self.r_name, on=True) try: input(" ----- ROBOT ENABLED, PLEASE PRESS 'ENTER' TO CONTINUE ----- ") except Exception: print("") # ENABLE GREEN LIGHT WHEN PROGRAM IS RUNNING self.l.set_light_state(self.r_name, on=False) self.l.set_light_state(self.l_name, on=True) # ENSURE THE WAYPOINTS LIST IS CLEARED AT TERMINATION OF PROGRAM def clean_shutdown(self): print("Stopping arm...") try: self.STOP = True self.move(point_list = None) self.l.set_light_state(self.l_name, on=False) except: print("There may have been an error exiting") print("Stop successful, exiting...") return # INPUT THE SIMULATED TRAJECTORY AND PUSH TO OUTPUT TO move() def sim_drone(self): M = self.parser.inputMatrix() point_list = list() for x in range(0,350,3): # TIME IN FLIGHT SIM = 0.1*RANGE point = [ M[x][2][0],-M[x][2][1],-M[x][2][2],-M[x][3][0],M[x][3][1],M[x][3][2] ] #point = [ M[x][2][0],-M[x][2][1],-M[x][2][2],0,0,0 ] point_list.append(point) print(point) self.move(wait=True, point_list=point_list, MAX_SPD_RATIO=0.8, MAX_JOINT_ACCL=[10.0, 8.0, 10.0, 10.0, 12.0, 12.0, 12.0]) # CONTAINS WAYPOINTS TO TRACE BOX def trace_box(self): print("I am tracing a box") point_list = list() point_list.append([0.0, 0.25, 0.0, 0.0, 0.0, 0.0]) self.move(point_list=point_list) point_list = list() point_list.append([0.0, 0.25, 0.1, 0.0, 0.0, 0.0]) self.move(point_list=point_list) point_list = list() point_list.append([0.0, -0.25, 0.1, 0.0, 0.0, 0.0]) self.move(point_list=point_list) point_list = list() point_list.append([0.0, -0.25, -0.1, 0.0, 0.0, 0.0]) self.move(point_list=point_list) point_list = list() point_list.append([0.0, 0.25, -0.1, 0.0, 0.0, 0.0]) self.move(point_list=point_list) point_list = list() point_list.append([0.0, 0.25, 0.0, 0.0, 0.0, 0.0]) self.move(point_list=point_list) return # MOVE TO ORIGIN DEFINED IN move() def moveToNeutral(self): print("\n --- Returning to neutral position (0.65, 0.0, 0.5, 0.0, 0.0, 0.0) ---") point = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] point_list = [point] success = self.move(wait=True, point_list=point_list, MAX_SPD_RATIO=0.2) return success # FUNCTION TO ABSTRACT CONTORL OF ARM def move(self, point_list = None, wait = True, MAX_SPD_RATIO=0.4, MAX_JOINT_ACCL=[7.0, 5.0, 8.0, 8.0, 8.0, 8.0, 8.0]): # one point in point_list = [x_coord, y_coord, z_coord, x_deg, y_deg, z_deg] try: limb = Limb() # point_list = [pointA, pointB, pointC, ...] traj_options = TrajectoryOptions() traj_options.interpolation_type = TrajectoryOptions.JOINT traj = MotionTrajectory(trajectory_options=traj_options, limb=limb) if self.STOP: traj.stop_trajectory() return True wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=MAX_SPD_RATIO, max_joint_accel=MAX_JOINT_ACCL) waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb) angles = limb.joint_ordered_angles() waypoint.set_joint_angles(joint_angles=angles) traj.append_waypoint(waypoint.to_msg()) for point in point_list: #q_base = quaternion_from_euler(0, math.pi/2, 0) q_base = quaternion_from_euler(0, 0, 0) #q_rot = quaternion_from_euler(math.radians(point[3]), math.radians(point[4]), math.radians(point[5])) # USE THIS IF ANGLES ARE IN DEGREES q_rot = quaternion_from_euler(point[3], point[4], point[5]) # USE THIS IF ANGLES ARE IN RADIANS q = quaternion_multiply(q_rot, q_base) # DEFINE ORIGIN origin = { 'x' : 0.65, 'y' : 0.0, 'z' : 0.4 } # CREATE CARTESIAN POSE FOR IK REQUEST newPose = PoseStamped() newPose.header = Header(stamp=rospy.Time.now(), frame_id='base') newPose.pose.position.x = point[0] + origin['x'] newPose.pose.position.y = point[1] + origin['y'] newPose.pose.position.z = point[2] + origin['z'] newPose.pose.orientation.x = q[0] newPose.pose.orientation.y = q[1] newPose.pose.orientation.z = q[2] newPose.pose.orientation.w = q[3] # REQUEST IK SERVICE FROM ROS ns = "ExternalTools/right/PositionKinematicsNode/IKService" iksvc = rospy.ServiceProxy(ns, SolvePositionIK) ikreq = SolvePositionIKRequest() # SET THE NEW POSE AS THE IK REQUEST ikreq.pose_stamp.append(newPose) ikreq.tip_names.append('right_hand') # ATTEMPT TO CALL THE SERVICE try: rospy.wait_for_service(ns, 5.0) resp = iksvc(ikreq) except: print("IK SERVICE CALL FAILED") return # HANDLE RETURN if (resp.result_type[0] > 0): joint_angles = resp.joints[0].position # APPEND JOINT ANGLES TO NEW WAYPOINT waypoint.set_joint_angles(joint_angles=joint_angles) traj.append_waypoint(waypoint.to_msg()) else: print("INVALID POSE") print(resp.result_type[0]) if(wait): print(" \n --- Sending trajectory and waiting for finish --- \n") result = traj.send_trajectory(wait_for_result=wait) if result is None: rospy.logerr('Trajectory FAILED to send') success = False elif result.result: rospy.loginfo('Motion controller successfully finished the trajcetory') success = True else: rospy.logerr('Motion controller failed to complete the trajectory. Error: %s', result.errorId) success = False else: print("\n --- Sending trajector w/out waiting --- \n") traj.send_trajectory(wait_for_result=wait) success = True return success except rospy.ROSInterruptException: print("User interrupt detected. Exiting before trajectory completes") # MAIN CONTROL LOOP def fly(self, weather="calm"): print("Flying") self.moveToNeutral() if self.args["reset"]: self.l.set_light_state(self.l_name, on=False) return if self.args["box"]: self.trace_box() self.moveToNeutral() self.sim_drone() #self.move(wait=True, point_list=[[0,0,0,1.159/2,0,0]]) #rate = rospy.Rate(RATE) #while not rospy.is_shutdown(): # rate.sleep() self.l.set_light_state(self.l_name, on=False) return