class moveTbot3: def __init__(self): rospy.init_node('move_turtle', anonymous=True) self.actions = String() self.pose = Pose() self.vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) self.action_subscriber = rospy.Subscriber('/actions', String, self.callback_actions) self.pid_subscriber = rospy.Subscriber("/Controller_Status", String, self.callback_pid) self.pose_subscriber = rospy.Subscriber('/odom', Odometry, self.pose_callback) self.status_publisher = rospy.Publisher("/status", String, queue_size=10) self.free = String(data="Idle") self.rate = rospy.Rate(1) print "Ready!" rospy.spin() def callback_pid(self, data): if data.data == "Done": if len(self.actions) > 0: self.execute_next() def callback_actions(self, data): self.actions = data.data.split("_") self.rate.sleep() self.execute_next() # self.move() def execute_next(self): action = self.actions.pop(0) direction = None if action == "MoveF" or action == "MoveB": current_pose = self.pose quat = (current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-math.pi / 4.0) and current_yaw < (math.pi / 4.0): print "Case 1" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 4.0) and current_yaw < (3.0 * math.pi / 4.0): print "Case 2" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 #direction = 'y' #decr x co elif current_yaw > (-3.0 * math.pi / 4.0) and current_yaw < (-math.pi / 4.0): print "Case 3" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 #direction = '-y' else: print "Case 4" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 #direction = '-x' PID(target_pose, "linear").publish_velocity() elif action == "TurnCW" or action == "TurnCCW": current_pose = self.pose quat = (current_pose.orientation.x, current_pose.orientation.y, current_pose.orientation.z, current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) yaw = euler[2] if action == "TurnCW": target_yaw = yaw - (math.pi / 2.0) if target_yaw < -math.pi: target_yaw += (math.pi * 2) else: target_yaw = yaw + (math.pi / 2.0) if target_yaw >= (math.pi): target_yaw -= (math.pi * 2) target_pose = Pose() target_pose.position = current_pose.position target_quat = Quaternion(*tf.transformations.quaternion_from_euler( euler[0], euler[1], target_yaw)) target_pose.orientation = target_quat print target_pose.orientation PID(target_pose, "rotational").publish_velocity() else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free) def pose_callback(self, data): self.pose = data.pose.pose
class moveTbot3: def __init__(self): rospy.init_node('move_turtle',anonymous=True, disable_signals=False) self.actions = String() self.pose = Pose() self.vel_pub = rospy.Publisher('/cmd_vel',Twist,queue_size=10) self.action_subscriber = rospy.Subscriber('/actions',String,self.callback_actions) self.pid_subscriber = rospy.Subscriber("/Controller_Status",String,self.callback_pid) self.pose_subscriber = rospy.Subscriber('/odom',Odometry,self.pose_callback) self.target_publisher = rospy.Publisher("/target_pose",Pose,queue_size = 1) self.status_publisher = rospy.Publisher("/status",String,queue_size = 10) self.orientation_raw = rospy.Publisher('/orientation',Float64,queue_size = 5) self.helper = Helper() self.init_state = self.helper.get_initial_state() self.current_state = copy.deepcopy(self.init_state) self.free = String(data = "next") self.rate = rospy.Rate(30) self.last_state = None print("Ready!") rospy.spin() def round_to_state(self,x): return round(x*2)/2.0 def callback_pid(self,data): if data.data == "Done": if len(self.actions)>0: self.execute_next() else: print("Shutting down") rospy.signal_shutdown("Done") def callback_actions(self,data): self.actions = data.data.split("_") self.rate.sleep() self.execute_next() # self.move() def execute_next(self): action = self.actions.pop(0) direction = None if action in self.helper.get_actions(): next_states = self.helper.get_successor(self.current_state) for next_action in next_states.keys(): if next_action == action: next_state = next_states[next_action][0] break print "current state: {}".format(self.current_state) print "next state: {}".format(next_state) goal_x = next_state.x goal_y = next_state.y if next_state.orientation == "EAST": goal_angel = 0 elif next_state.orientation == "NORTH": goal_angel = math.pi / 2.0 elif next_state.orientation == "WEST": goal_angel = math.pi else: goal_angel = -math.pi/2.0 current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) current_euler = tf.transformations.euler_from_quaternion(quat) target_quat = Quaternion(*tf.transformations.quaternion_from_euler(current_euler[0],current_euler[1],goal_angel)) target_pose = copy.deepcopy(current_pose) target_euler = [current_euler[0],current_euler[1],goal_angel] #target_pose.position.x = goal_x #target_pose.position.y = goal_y #target_pose.orientation = target_quat target_pose = [goal_x,goal_y,goal_angel] #self.target_publisher.publish(target_pose) if action in ["MoveF", "MoveB"]: PID(target_pose,"linear").publish_velocity() else: PID(target_pose,"rotational").publish_velocity() # self.current_state = next_state else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free) def pose_callback(self,data): self.pose = data.pose.pose #print "Current Pose:"+str(self.pose.position.x)+','+str(self.pose.position.y) state_x = self.round_to_state(self.pose.position.x) state_y = self.round_to_state(self.pose.position.y) quat = (self.pose.orientation.x,self.pose.orientation.y,self.pose.orientation.z, self.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) z_angel = euler[2] #print','+str(z_angel) if z_angel >= -math.pi/4.0 and z_angel < math.pi/4.0: state_rot = "EAST" elif z_angel >= math.pi/4.0 and z_angel < 3.0 * math.pi /4.0: state_rot = "NORTH" elif z_angel >= -3.0 * math.pi/ 4.0 and z_angel < -math.pi/4.0: state_rot = "SOUTH" else: state_rot = "WEST" orientation = Float64() orientation.data = z_angel self.orientation_raw.publish(orientation) self.current_state = State(state_x,state_y,state_rot)
class movePR2: def __init__(self): rospy.init_node('move_pr2',anonymous = True) self.actions = String() self.pose = Pose() self.vel_pub = rospy.Publisher('/base_controller/command',Twist,queue_size=10) self.action_subscriber = rospy.Subscriber('/actions',String,self.callback_actions) self.pid_subscriber = rospy.Subscriber("/Controller_Status",String,self.callback_pid) self.pose_subscriber = rospy.Subscriber('/base_odometry/odom',Odometry,self.pose_callback) self.status_publisher = rospy.Publisher("/status",String,queue_size = 10) self.sense_publisher = rospy.Publisher("/sense_report", String, queue_size = 10) self.free = String(data = "Idle") self.rate = rospy.Rate(30) moveit_commander.roscpp_initialize(sys.argv) self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.group_name = "right_arm" self.group = moveit_commander.MoveGroupCommander(self.group_name) self.standing_joints = [-0.00020377586883224552, \ 0.03213652755729335, \ -0.006529160981967763, \ -0.1942282176898953, \ -0.0024028167806875445, \ -0.20476869150849364, \ 0.0002324956593149352] self.gripper_pub = rospy.Publisher("r_gripper_controller/command", Pr2GripperCommand, queue_size=10) while self.gripper_pub.get_num_connections() == 0: rospy.loginfo("Waiting for gripper publisher to connect") rospy.sleep(1) self.headpub = rospy.Publisher("/head_traj_controller/point_head_action/goal", PointHeadActionGoal, queue_size = 10) while self.headpub.get_num_connections() == 0: rospy.loginfo("Waiting for head publisher to connect") rospy.sleep(1) print "Ready!" rospy.spin() def callback_pid(self,data): if data.data == "Done": if len(self.actions)>0: self.execute_next() def callback_actions(self,data): self.actions = data.data.split("_") self.rate.sleep() self.execute_next() # self.move() def execute_next(self): action = self.actions.pop(0) direction = None if action == "MoveF" or action == "MoveB": current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-math.pi /4.0) and current_yaw < (math.pi / 4.0): print "Case 1" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 4.0 ) and current_yaw < (3.0 * math.pi / 4.0): print "Case 2" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 #direction = 'y' #decr x co elif current_yaw > (-3.0*math.pi /4.0) and current_yaw < (-math.pi /4.0): print "Case 3" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 #direction = '-y' else: print "Case 4" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 #direction = '-x' PID(target_pose,"linear").publish_velocity() elif action == "TurnCW" or action == "TurnCCW": current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) yaw = euler[2] if action == "TurnCW": target_yaw = yaw - ( math.pi / 2.0) if target_yaw < -math.pi: target_yaw += (math.pi * 2) else: target_yaw = yaw + ( math.pi / 2.0) if target_yaw >= (math.pi ): target_yaw -= (math.pi * 2 ) target_pose = Pose() target_pose.position = current_pose.position target_quat = Quaternion(*tf.transformations.quaternion_from_euler(euler[0],euler[1],target_yaw)) target_pose.orientation = target_quat print target_pose.orientation PID(target_pose,"rotational").publish_velocity() elif action == "pick": self.pick() elif action == "place": self.place() elif "sense" in action: obj_name = action.split("|")[1] obj_name = obj_name.replace("@","_") print("Object: " + obj_name) self.sense(obj_name) else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free) def place(self): print("Performing Place Operation") print("Step 1: Opening Gripper!") self.gripper("open") print("Step 2: Closing Gripper!") self.gripper("close") def pick(self): print("Performing Pick Operation...") print("Step 1: Opening Gripper") self.gripper("open") print("Step 2: Moving To Approach") approach_joints = [-0.3039317534057506,\ 0.854805322462882,\ -1.5700910376892132,\ -0.16689570952151023,\ -0.14461307158717318,\ -1.3447341945775477,\ 0.7118105483210035] joint_goal = approach_joints self.group.go(joint_goal, wait=True) self.group.stop() rospy.sleep(1) print("Step 3: Completing Grasp") grasp_joints = [-0.03260850653005498,\ 0.8679200984865636,\ -1.5700975812614626,\ -0.18182010402644977,\ -0.017845654757860707,\ -1.0650192634362217,\ 0.7383857717301172] joint_goal = grasp_joints self.group.go(joint_goal, wait=True) self.group.stop() rospy.sleep(1) print("Step 4: Closing Gripper") self.gripper("close") print("Step 5: Returning to Standing") self.reset_joints() def gripper(self,cmd): if(cmd == "open"): print("Opening Gripper") data = Pr2GripperCommand() data.position = 0.1 data.max_effort= 100.0 self.gripper_pub.publish(data) rospy.sleep(1) elif(cmd == "close"): print("Closing Gripper") data = Pr2GripperCommand() data.position = 0.0 data.max_effort= 100.0 self.gripper_pub.publish(data) rospy.sleep(1) def reset_joints(self): print("Going to Standing Stance") joint_goal = self.standing_joints self.group.go(joint_goal, wait=True) self.group.stop() rospy.sleep(1) current_joints = self.group.get_current_joint_values() return current_joints def sense(self, obj_name): self.look_at(obj_name) def pose_callback(self,data): self.pose = data.pose.pose def look_at(self, obj_name, frame_id='base_link'): """ This method will publish coke_location coordinates to PR2's head. This will rotate PR2's head to point to those coordinates in the environment. :param: frame_id: str :param: x: float :param: y: float :param: z: float """ print("Trying to look at {}".format(obj_name)) gms_func = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) can_pose = gms_func(obj_name, "pr2::base_footprint").pose.position data = PointHeadActionGoal() point = PointStamped() point.header.frame_id = frame_id point.point.x = can_pose.x point.point.y = can_pose.y point.point.z = can_pose.z data.goal = PointHeadGoal() data.goal.target = point data.goal.pointing_frame = "high_def_frame" data.goal.pointing_axis.x = 1 data.goal.pointing_axis.y = 0 data.goal.pointing_axis.z = 0 # Publish goal to PR2's head self.headpub.publish(data) print ('Moving head to coordinates: ') print (can_pose) # The callback method for this subscriber will only be activated when the head is pointing to the goal. self.headsub = rospy.Subscriber("/head_traj_controller/point_head_action/result", PointHeadActionResult, self.callback_sense) def callback_sense(self, status): print ('Callback action') point = PointHeadActionResult() if point.status.status == 0: self.headsub.unregister() time.sleep(2) classifier = ObjectClassifier() classifier.sense() return
class moveDrone: def init_drone(self): connection_string = '127.0.0.1:14540' MAV_MODE_AUTO = 4 print "Connecting" self.vehicle = connect(connection_string, wait_ready=True) def PX4setMode(mavMode): self.vehicle._master.mav.command_long_send( self.vehicle._master.target_system, self.vehicle._master.target_component, mavutil.mavlink.MAV_CMD_DO_SET_MODE, 0, mavMode, 0, 0, 0, 0, 0, 0) #Create a message listener for self.home position fix @self.vehicle.on_message('HOME_POSITION') def listener(self, name, home_position): global home_position_set self.home_position_set = True print "Home Location Set %s" % self.vehicle.home_location # Display basic self.vehicle state #print " Type: %s" % self.vehicle.vehicle_type print " Armed: %s" % self.vehicle.armed print " System status: %s" % self.vehicle.system_status.state print " GPS: %s" % self.vehicle.gps_0 print " Alt: %s" % self.vehicle.location.global_relative_frame.alt # Change to AUTO mode PX4setMode(MAV_MODE_AUTO) time.sleep(1) # Load commands cmds = self.vehicle.commands cmds.clear() self.home = self.vehicle.location.global_relative_frame #takeoff to starting point wp = get_location_offset_meters(self.home, 0, 0, 10) cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt) cmds.add(cmd) wp = get_location_offset_meters( self.home, *get_offsets(self.grid, self.loc.x, self.loc.y)) cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt) cmds.add(cmd) # Upload mission cmds.upload() time.sleep(2) # Arm self.vehicle self.vehicle.armed = True # monitor mission execution nextwaypoint = self.vehicle.commands.next while nextwaypoint < len(self.vehicle.commands): if self.vehicle.commands.next > nextwaypoint: display_seq = self.vehicle.commands.next + 1 print "Moving to waypoint %s" % display_seq nextwaypoint = self.vehicle.commands.next time.sleep(1) def __init__(self): rospy.init_node('move_drone', anonymous=True) self.actions = String() self.helper = api.Helper() self.loc = self.helper.get_initial_state() self.action_subscriber = rospy.Subscriber('/actions', String, self.callback_actions) self.status_publisher = rospy.Publisher("/status", String, queue_size=10) self.free = String(data="next") self.rate = rospy.Rate(30) self.root_path = os.path.abspath( os.path.join(os.path.dirname(__file__), os.path.pardir)) with open(self.root_path + '/reef.json', 'r') as f: self.grid = json.load(f) self.init_drone() print("Ready!") rospy.spin() def callback_actions(self, data): self.actions = data.data.split("_") self.rate.sleep() for i in range(len(self.actions)): self.execute_next() time.sleep(5) def execute_next(self): action = self.actions.pop(0) if action == "MoveF" or action == "MoveB": current_loc = self.loc deltas = { 'MoveF': { 'NORTH': (0, 1), 'SOUTH': (0, -1), 'EAST': (1, 0), 'WEST': (-1, 0) }, 'MoveB': { 'NORTH': (0, -1), 'SOUTH': (0, 1), 'EAST': (-1, 0), 'WEST': (1, 0) } } new_loc = api.State(*([ sum(dim) for dim in zip((self.loc.x, self.loc.y ), deltas[action][self.loc.direction]) ] + [self.loc.direction])) # Load commands cmds = self.vehicle.commands #cmds.clear() if 0 <= new_loc.x < 6 and 0 <= new_loc.y < 4: print "Taking action %s to go from %s to %s\n" % ( action, current_loc, new_loc) self.loc = new_loc wp = get_location_offset_meters( self.home, *get_offsets(self.grid, self.loc.x, self.loc.y)) cmd = Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 0, 1, 0, 0, 0, 0, wp.lat, wp.lon, wp.alt) cmds.add(cmd) cmds.upload() time.sleep(2) else: print "Attempted to move out of grid.\n" elif action == "TurnCW" or action == "TurnCCW": res_orientations = { 'NORTH': { 'TurnCW': 'EAST', 'TurnCCW': 'WEST' }, 'SOUTH': { 'TurnCW': 'WEST', 'TurnCCW': 'EAST' }, 'EAST': { 'TurnCW': 'SOUTH', 'TurnCCW': 'NORTH' }, 'WEST': { 'TurnCW': 'NORTH', 'TurnCCW': 'SOUTH' } } new_loc = api.State(self.loc.x, self.loc.y, res_orientations[self.loc.direction][action]) print "Taking action %s to go from %s to %s" % (action, self.loc, new_loc) self.loc = new_loc wp = get_location_offset_meters( self.home, *get_offsets(self.grid, self.loc.x, self.loc.y)) Command(0, 0, 0, mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT, mavutil.mavlink.MAV_CMD_CONDITION_YAW, 0, 1, 90, 0, 1 if action == "TurnCW" else -1, 1, wp.lat, wp.lon, wp.alt) else: print "Invalid action" exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free)
class moveTbot3: def __init__(self): self.model_state_publisher = rospy.Publisher("/gazebo/set_model_state",ModelState,queue_size = 10) rospy.init_node('move_turtle',anonymous = True) self.actions = String() self.pose = Pose() self.vel_pub = rospy.Publisher('/cmd_vel',Twist,queue_size=10) self.action_subscriber = rospy.Subscriber('/actions',String,self.callback_actions) self.pid_subscriber = rospy.Subscriber("/Controller_Status",String,self.callback_pid) self.pose_subscriber = rospy.Subscriber('/odom',Odometry,self.pose_callback) self.status_publisher = rospy.Publisher("/status",String,queue_size = 10) self.free = String(data = "Idle") self.rate = rospy.Rate(30) print "Ready!" rospy.spin() def spawn_can(self,posx, posy): global i_d i_d+=1 f_read = open(os.path.expanduser("~") + "/.gazebo/models/fuelstation/model-2.sdf",'r') sdff = f_read.read() f_read.close() rospy.wait_for_service('gazebo/spawn_sdf_model') spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel) pose = Pose() pose.position.x = posx pose.position.y = posy pose.position.z = 0 spawn_model_prox("fuelstat{}".format(i_d), sdff, "robots_name_space", pose, "world") def callback_pid(self,data): if data.data == "Done": if len(self.actions)>0: self.execute_next() def callback_actions(self,data): self.actions = data.data.split("_") self.rate.sleep() self.execute_next() # self.move() def execute_next(self): action = self.actions.pop(0) direction = None if action == "REFUEL": #add changing color of fuel can print "Recharged!" current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] ''' approx pos to nearest 0.5 ''' fuel_can_pose_x = round(current_pose.position.x*2.0)/2 fuel_can_pose_y = round(current_pose.position.y*2.0)/2 print("HELLLOOOO") self.spawn_can(fuel_can_pose_x,fuel_can_pose_y) PID(current_pose,"linear").publish_velocity() elif action == "MoveF": current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-1.0* math.pi /8.0) and current_yaw < (math.pi / 8.0): print "Case 1 : East" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 8.0 ) and current_yaw < (3.0 * math.pi / 8.0): print "Case 2: North East" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 target_pose.position.x += 0.5 #direction = 'y' #decr x co elif current_yaw > (3.0*math.pi /8.0) and current_yaw < (5.0*math.pi /8.0): print "Case 3:North" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 #direction = '-y' elif current_yaw > (5 * math.pi / 8.0) and current_yaw < (7.0 * math.pi / 8.0): # Facing SOUTH EAST print "Case 4: North West" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 target_pose.position.y += 0.5 elif current_yaw > (-3.0 * math.pi / 8.0) and current_yaw < (-1.0 * math.pi / 8.0): # Facing SOUTH EAST print "Case 5: South East" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 target_pose.position.y -= 0.5 #direction = '-x' elif current_yaw > (-5.0 * math.pi / 8.0) and current_yaw < (-3.0 * math.pi / 8.0): # Facing SOUTH print "Case 6: South" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 elif current_yaw > (-7.0 * math.pi / 8.0) and current_yaw < (-5.0 * math.pi / 8.0): # Facing SOUTH WEST print "Case 7: South West" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 target_pose.position.y -= 0.5 else: print "Case 8: West" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 PID(target_pose,"linear").publish_velocity() elif action == "MoveB": current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) current_yaw = euler[2] if current_yaw > (-1.0* math.pi /8.0) and current_yaw < (math.pi / 8.0): print "Case 1 : East" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 #direction = 'x' #incr y co-ordinate elif current_yaw > (math.pi / 8.0 ) and current_yaw < (3.0 * math.pi / 8.0): print "Case 2: North East" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 target_pose.position.x -= 0.5 #direction = 'y' #decr x co elif current_yaw > (3.0*math.pi /8.0) and current_yaw < (5.0*math.pi /8.0): print "Case 3:North" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y -= 0.5 #direction = '-y' elif current_yaw > (5 * math.pi / 8.0) and current_yaw < (7.0 * math.pi / 8.0): # Facing SOUTH EAST print "Case 4: North West" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 target_pose.position.y -= 0.5 elif current_yaw > (-3.0 * math.pi / 8.0) and current_yaw < (-1.0 * math.pi / 8.0): # Facing SOUTH EAST print "Case 5: South East" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x -= 0.5 target_pose.position.y += 0.5 #direction = '-x' elif current_yaw > (-5.0 * math.pi / 8.0) and current_yaw < (-3.0 * math.pi / 8.0): # Facing SOUTH print "Case 6: South" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.y += 0.5 elif current_yaw > (-7.0 * math.pi / 8.0) and current_yaw < (-5.0 * math.pi / 8.0): # Facing SOUTH WEST print "Case 7: South West" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 target_pose.position.y += 0.5 else: print "Case 8: West" #raw_input() target_pose = copy.deepcopy(current_pose) target_pose.position.x += 0.5 PID(target_pose,"linear").publish_velocity() elif action == "TurnCW" or action == "TurnCCW": print "Case: Turning" current_pose = self.pose quat = (current_pose.orientation.x,current_pose.orientation.y,current_pose.orientation.z,current_pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quat) yaw = euler[2] if action == "TurnCW": target_yaw = yaw - ( math.pi / 4.0) if target_yaw < -math.pi: target_yaw += (math.pi * 2) else: target_yaw = yaw + ( math.pi / 4.0) if target_yaw >= (math.pi ): target_yaw -= (math.pi * 2 ) target_pose = Pose() target_pose.position = current_pose.position target_quat = Quaternion(*tf.transformations.quaternion_from_euler(euler[0],euler[1],target_yaw)) target_pose.orientation = target_quat print target_pose.orientation PID(target_pose,"rotational").publish_velocity() else: print "Invalid action " + action exit(-1) if len(self.actions) == 0: self.status_publisher.publish(self.free) def pose_callback(self,data): self.pose = data.pose.pose