def get_closest_poles(self, poles):
p0 = None
p1 = None
p2 = None
d1 = np.inf
d2 = np.inf
#finds 2 closest poles
for pole in poles:
other_poles = poles
other_poles.remove(pole)
for other_pole in other_poles:
if self.dist_two_poles(pole, other_pole) < d1:
p0 = pole
p1 = other_pole
d1 = self.dist_two_poles(pole, other_pole)
#finds 3rd closest poles
other_poles = poles
other_poles.remove(p0)
other_poles.remove(p1)
closest_from_pair = None
for pole in other_poles:
if self.dist_two_poles(pole, p0) < d2:
p3 = pole
closest_from_pair = p0
d2 = self.dist_two_poles(pole, p0)
if self.dist_two_poles(pole, p1) < d2:
p3 = pole
closest_from_pair = p1
d2 = self.dist_two_poles(pole, p1)
#switch p0 and p1 if they were assigned incorrectly
if closest_from_pair == p0:
p0 = p1
p1 = closest_from_pair
rosprint([p0, p1, p2])
return [p0, p1, p2]
def __init__(self):
rosprint("Initialised move server!")
self.server = actionlib.SimpleActionServer("move", MoveAction,
self.execute, False)
self.server.start()
#publishers
self.move_pub = rospy.Publisher("cmd_move", CmdMove, queue_size=1000)
self.feedback = MoveFeedback()
self.result = MoveResult()
def __init__(self):
rospy.init_node("move_node", anonymous=True)
rosprint("Initialised move node!")
self.move_sub = rospy.Subscriber("cmd_move", CmdMove, self.move)
self.vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1000)
rospy.spin()
def dist_two_poles(self, pole0, pole1):
d = 0
z0 = self.dist_to_robot(pole0)
z1 = self.dist_to_robot(pole1)
phi = np.arctan(pole0.y / pole0.x) - np.arctan(pole1.y / pole1.x)
#phi = np.pi/2 - np.arctan(pole0.x/pole0.y) - np.arctan(pole1.x/pole1.y)
d = np.sqrt(z0 * z0 + z1 * z1 - 2 * z0 * z1 * np.cos(phi))
rosprint("d1:{},d2:{},phi:{}".format(z0, z1, phi))
return d
def __init__(self):
self.referee_pub_status = rospy.Publisher("waitForTeams",
waitForTeams,
queue_size=10)
self.referee_pub_control = rospy.Publisher("gameControl",
gameControl,
queue_size=10)
rospy.init_node("referee_node", anonymous=True)
rosprint("Referee_node initialised!")
def build_map(self):
#close_poles = self.get_closest_poles(self.extract_poles())
close_poles = self.extract_poles()
map_unit = self.get_map_unit(close_poles)
if map_unit != 0:
rosprint("Detected map unit:{}".format(map_unit))
rosprint("The map dimensions are:{},{}".format(
map_unit * 3, map_unit * 5))
return True
def __init__(self):
rosprint("Initialised build_map server!")
self.map_init = False
self.det_objs = None
self.server = actionlib.SimpleActionServer("build_map", BuildMapAction,
self.execute, False)
self.server.start()
self.feedback = BuildMapFeedback()
self.detected_objs_sub = rospy.Subscriber("detected_objs",
DetectedObjs,
self.det_objs_cb)
self.OR_pub = rospy.Publisher("OR_execution", Bool, queue_size=1)
self.map_init = False
self.det_objs = None
def match(self, scanned_objs_message):
"""
TODO: make this a lot better after the fixing the laser and camera
"""
matches = []
camera_msg = self.current_camera_msg
if camera_msg is not None and self.sim_env is False:
#rospy.loginfo(camera_msg.kinectObjList)
for laser_object in scanned_objs_message.scannedObjList:
laser_coords = np.array((laser_object.x, laser_object.y))
for kinect_object in camera_msg.kinectObjList:
match_obj = DetectedObj()
kinect_coords = np.array(
(kinect_object.x, kinect_object.y))
distance = np.linalg.norm(laser_coords - kinect_coords)
match_obj.x = kinect_object.x
match_obj.y = kinect_object.y
if kinect_object.color == "G":
#rosprint(distance)
if distance < 0.2:
match_obj.id = "pole"
if match_obj not in matches:
matches.append(match_obj)
if kinect_object.z < -0.25 and kinect_object.color != "G":
match_obj.id = "{}_goal".format(kinect_object.color)
if match_obj not in matches:
d = np.sqrt(kinect_object.x * kinect_object.x +
kinect_object.y * kinect_object.y)
if self.own_goal_det is False:
rosprint(
"Detected our own goal, color:{}".format(
kinect_object.color))
self.own_goal_det = True
if d > 2.0 and self.other_goal_det is False:
rosprint("Detected other teams goal, color:{}".
format(kinect_object.color))
self.other_goal_det = True
matches.append(match_obj)
elif kinect_object.color != "G":
match_obj.id = "{}_puck".format(kinect_object.color)
if match_obj not in matches:
matches.append(match_obj)
self.current_camera_msg = None
#rosprint("================================================================")
#rospy.loginfo("objects matched at: {}".format(matches))
#rosprint("================================================================")
self.publish_matches(matches, camera_msg)
def execute(self, goal):
r = rospy.Rate(1)
sucess = True
#publish info to console
rosprint("Executing move in %s dir with %i duration and %i speed." %
(goal.direction, goal.duration, goal.speed))
#execute action
self.move(goal.direction, goal.duration, goal.speed)
rospy.sleep(goal.duration)
self.move("stop")
#r.sleep()
#publish result
self.result.message = "succeeded"
rosprint("Move successfuly executed!")
self.server.set_succeeded(self.result)
def __init__(self, queue_size=1000):
rospy.init_node("tf_node")
rosprint("Initialised transform node!")
# self.laser_sub = rospy.Subscriber("front_laser/scan",LaserScan,
# self.laser_sub_cb)
self.depth_sub = rospy.Subscriber("kinect_objs", KinectObjs,
self.depth_sub_cb)
# self.laser_pub = rospy.Publisher("laser_top_shield", LaserScan,queue_size)
self.depth_pub = rospy.Publisher("kinect_objs_transformed",
KinectObjs,
queue_size=1)
self.tf_buf = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buf)
rospy.spin()
def execute(self, goal):
#rospy.loginfo("Executing build map!")
result = BuildMapResult()
obstacle = False
detect_stuff = Bool()
detect_stuff.data = 1
self.OR_pub.publish(detect_stuff)
if len(self.extract_poles()) >= 3:
rosprint("Three poles detected! Trying to build map!")
if self.build_map() is True:
self.map_init = True
rosprint("Built map succesfully!")
self.feedback.message = "something"
self.server.publish_feedback(self.feedback)
if self.map_init is True:
result.message = "succeeded"
self.server.set_succeeded(result)
else:
result.message = "build_map_failed"
self.server.set_aborted(result)
def __init__(self):
rospy.init_node("odom_node")
rosprint("Initialised odom node!")
self.delta_pose_pub = rospy.Publisher("pose_delta",
DeltaPose,
queue_size=1000)
self.scanned_sub = rospy.Subscriber("kinect_objs", KinectObjs,
self.update_pose)
self.last_time_stamp = rospy.Time.now()
self.last_kinect_msg = None
self.last_v = None
self.delta_x = 0
self.delta_y = 0
self.delta_t = 0
self.delta_phi = 0
rospy.spin()
def avoid_obstacle(self, distance=None):
"""
Sends CmdMove message to cmd_move topic. Direction: "fwd" = forward, "cw" = clockwise, "ccw" = counterclockwise. Duration in seconds. Speed in m/s.
"""
if distance > 0:
detected_obj = self.laser.obstacle_position(distance)
phi_view = 30
for data in detected_obj:
range_obj, phi_obj = data
if abs(phi_obj) < phi_view:
if phi_obj > 0:
rosprint("Avoiding obstacle, turning right!")
self.move("cw", 1, 1)
else:
rosprint("Avoiding obstacle, turning left!")
self.move("ccw", 1, 1)
rosprint("No obstacle ahead! Moving randomly!")
if rd.random() < 0.66:
self.move("fwd", 1, 0.1)
elif rd.random() < 0.81:
self.move("cw", 1, 0.5)
else:
self.move("ccw", 1, 0.5)
else:
raise ValueError("Obstacle distance can't be negative!\n")
def get_map_unit(self, close_poles):
if close_poles is not None:
map_unit = 0
d1 = self.dist_two_poles(close_poles[0], close_poles[1])
d2 = self.dist_two_poles(close_poles[1], close_poles[2])
rosprint("The two pole dist:{},{}".format(d1, d2))
rosprint("Relation of the two poles:{}".format(d1 / d2))
rosprint(
"Deltas to the closest possible relations:{},{},{},{}".format(
d1 / d2 - 2 / 3.0, d1 / d2 - 3 / 5.0, d1 / d2 - 1 / 2.0,
d1 / d2 - 0.5 / 3.0))
if d1 == 0 or d2 == 0:
return map_unit
elif d1 / d2 - 2 / 3.0 < 0.05:
map_unit = d2 * 6.6666 / 5.0
elif d1 / d2 - 3 / 5.0 < 0.05:
map_unit = d2 * 4.0 / 5.0
elif d1 / d2 - 1 / 2.0 < 0.05:
map_unit = d2 * 4.0 / 5.0
elif d1 / d2 - 0.5 / 3.0 < 0.05:
map_unit = d2 / 3.0
return map_unit
def active_cb():
rosprint("Action is now active: build_map")
pass
def done_cb(state, result):
rosprint(
"Action done: build_map, finished in state {} with result {}".format(
state, result))
pass
#!/usr/bin/env python
import rospy
import actionlib
from player.msg import *
from player import rosprint
def move_client():
#setup client
client = actionlib.SimpleActionClient("move", MoveAction)
client.wait_for_server()
#cerate and send goal
#move_goal = MoveGoal()
#move_goal.direction = "ccw"
#move_goal.duration = 1
#move_goal.speed = 0.5
#client.send_goal(move_goal)
client.wait_for_result()
return client.get_result()
if __name__ == '__main__':
try:
rospy.init_node("move_client")
result = move_client()
except rospy.ROSInterruptException:
rosprint("Program interrupted before completion.")
if phi_obj > 0:
rosprint("Avoiding obstacle, turning right!")
self.move("cw", 1, 1)
else:
rosprint("Avoiding obstacle, turning left!")
self.move("ccw", 1, 1)
rosprint("No obstacle ahead! Moving randomly!")
if rd.random() < 0.66:
self.move("fwd", 1, 0.1)
elif rd.random() < 0.81:
self.move("cw", 1, 0.5)
else:
self.move("ccw", 1, 0.5)
else:
raise ValueError("Obstacle distance can't be negative!\n")
if __name__ == '__main__':
player = PlayerNode()
if player.trees_on is False:
rosprint(player.trees_on)
player.run_smach()
loop_rate = rospy.Rate(10)
while not rospy.is_shutdown():
# 10 Hz loop
loop_rate.sleep()
def feedback_cb(feedback):
rosprint("Feedback build_map from action: {}".format(feedback))
pass
