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