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
