def __init__(self, cf_id): self.cf_id = cf_id # Subscribed services self._services = { "emergency": None, "take_off": None, "hover": None, "land": None, "stop": None, "toggle_teleop": None, "set_param": None } self.pose = None self.initial_pose = None self.goals = { "goal": Position(), "formation": Position(), "trajectory": Position() } self._goal_publisher = None self.state = "" self._init_cf()
def publish_cmd(goal): cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "map" cmd.x = goal[0] cmd.y = goal[1] cmd.z = goal[2] pub_cmd.publish(cmd)
def main(): global pos, goal, detected rate = rospy.Rate(5) z_dist = 0.4 tol_takeOff = 0.05 tol2 = 0.1 takeOff = True spin = True cmd = Position() while not rospy.is_shutdown(): if takeOff == True: print("Take off") while math.sqrt((z_dist-pos['z'])**2) > tol_takeOff: cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "map" cmd.x = pos['x'] cmd.y = pos['y'] cmd.z = 0.4 cmd.yaw = pos['yaw'] pub_cmd.publish(cmd) rate.sleep() print("done") takeOff = False else: if detected == True: rospy.loginfo("detected") while math.sqrt((goal['x']-pos['x'])**2 + (goal['y']-pos['y'])**2 + (goal['z']-pos['z'])**2 ) > tol2: cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "map" cmd.x = goal['x'] cmd.y = goal['y'] cmd.z = goal['z'] cmd.yaw = goal['yaw'] pub_cmd.publish(cmd) rate.sleep() detected = False if(spin == True): rospy.sleep(1.5) checkpointspin_server() spin = False else: cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "map" pub_cmd.publish(cmd) rate.sleep()
def __init__(self, cf_list, min_dist, start_goal): self.abs_ctrl_mode = False self.scale = 1.0 self.formation = None self.extra_agents = [] self._min_dist = min_dist #: (float) Minimum distance between agents in formation self._cf_list = cf_list self._n_cf = len(cf_list) #: (int) Number of CF in the swarm self._pose_cnt = 0 #: (int) To know when compute pose self._rate = rospy.Rate(100) _initial_formation_goal = Position() #: Position: formation start position _initial_formation_goal.x = start_goal[0] _initial_formation_goal.y = start_goal[1] _initial_formation_goal.z = start_goal[2] _initial_formation_goal.yaw = start_goal[3] #: All possible formations self._formations = {"square": SquareFormation(self._min_dist), "v": VFormation(self._min_dist), "pyramid": PyramidFormation(self._min_dist), "circle": CircleFormation(self._min_dist), "line": LineFormation(self._min_dist),} # Publisher self._formation_pose_pub = rospy.Publisher('/formation_pose', Pose, queue_size=1) self._formation_goal_pub = rospy.Publisher('/formation_goal', Position, queue_size=1) self._formation_pose = Pose() self._formation_goal = Position() self._formation_goal_vel = Twist() self._formation_goal = _initial_formation_goal # Subscribers rospy.Subscriber("/formation_goal_vel", Twist, self._formation_goal_vel_handler) self._crazyflies = {} #: dict of list: Information of each CF # Initialize each CF for cf_id in cf_list: self._crazyflies[cf_id] = {"formation_goal": Position(), # msg "swarm_id": 0, # int, id in the swarm "formation_goal_pub": None, # publisher "initial_position": None} # Add goal publisher self._crazyflies[cf_id]["formation_goal_pub"] =\ rospy.Publisher('/%s/formation_goal' % cf_id, Position, queue_size=1) # Start services rospy.Service('/set_formation', SetFormation, self._set_formation) rospy.Service('/toggle_ctrl_mode', Empty, self._toggle_ctrl_mode) rospy.Service('/formation_inc_scale', Empty, self._formation_inc_scale) rospy.Service('/formation_dec_scale', Empty, self._formation_dec_scale) rospy.Service('/get_formations_list', GetFormationList, self._return_formation_list)
def __init__(self, prefix): self.prefix = prefix worldFrame = rospy.get_param("~worldFrame", "/world") self.rate = rospy.Rate(5) rospy.wait_for_service(prefix + '/update_params') rospy.loginfo("found update_params service") self.update_params = rospy.ServiceProxy(prefix + '/update_params', UpdateParams) self.setParam("kalman/resetEstimation", 1) self.setParam("flightmode/posSet", 1) self.pub = rospy.Publisher(prefix + "/cmd_setpoint", Position, queue_size=1) self.msg = Position() self.msg.header.seq = 0 self.msg.header.stamp = rospy.Time.now() self.msg.header.frame_id = worldFrame self.msg.x = 0 self.msg.y = 0 self.msg.z = 0 self.msg.yaw = 0 self.stop_pub = rospy.Publisher(prefix + "/cmd_stop", Empty, queue_size=1) self.stop_msg = Empty()
def __init__(self): # Pull ROS parameters from launch file: param = rospy.search_param("goal_topic") self.goal_topic = rospy.get_param(param) param = rospy.search_param("goal_frame") self.goal_frame = rospy.get_param(param) param = rospy.search_param("sim_or_real") self.sim_or_real = rospy.get_param(param) param = rospy.search_param("distance_forward") self.distance_forward = rospy.get_param(param) param = rospy.search_param("spin_count") self.spin_count = rospy.get_param(param) # Confirm sim or real is proper if not (self.sim_or_real == 'sim' or self.sim_or_real == 'real'): rate = rospy.Rate(1) while not rospy.is_shutdown(): rospy.loginfo( "sim_or_real mmust be set to either 'sim' or 'real'") rate.sleep() # Initialize goal message and publisher self.goal = Position() self.goal.header.frame_id = self.goal_frame self.pub_goal = rospy.Publisher(self.goal_topic, Position, queue_size=10) # Delay briefly for publisher to initialize rospy.sleep(1)
def compute_formation_positions(self): # (dX, dY) sign for each position in tier tier_poses_sign = [(-1, -1), (-1, 1), (1, 1), (1, -1)] for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Find tier information # i=0 -> tier=0, i=1,2,3,4 -> tier = 1, i=5,6,7,8 -> tier = 2 ... tier_num = ceil(i / 4.0) tier_pos = i % 4 # Position in the tier square_length = 2 * sin(self.theta) * self.tier_dist * tier_num # Compute formation position z_dist = -1 * tier_num * self.tier_dist x_dist = tier_poses_sign[tier_pos][0] * square_length / 2 y_dist = tier_poses_sign[tier_pos][1] * square_length / 2 # information from center center_dist, theta, center_height = compute_info_from_center( [x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height
def compute_formation_positions(self): for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Compute formation position z_dist = 0 if i == 0: x_dist = 0 y_dist = 0 else: angle = i * self.angle_between_agents x_dist = self._scale * cos(angle) y_dist = self._scale * sin(angle) # Compute information from center center_dist, theta, center_height = compute_info_from_center( [x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height return self._agents_goals
def _init_publishers(self): """Initialize all publishers""" self.cmd_vel_pub = rospy.Publisher(self.cf_id + '/cmd_vel', Twist, queue_size=1) self.cmd_vel_msg = Twist() self.cmd_hovering_pub = rospy.Publisher(self.cf_id + "/cmd_hovering", Hover, queue_size=1) self.cmd_hovering_msg = Hover() self.cmd_hovering_msg.header.seq = 0 self.cmd_hovering_msg.header.stamp = rospy.Time.now() self.cmd_hovering_msg.header.frame_id = self.world_frame self.cmd_hovering_msg.yawrate = 0 self.cmd_hovering_msg.vx = 0 self.cmd_hovering_msg.vy = 0 self.cmd_pos_pub = rospy.Publisher(self.cf_id + "/cmd_position", Position, queue_size=1) self.cmd_pos_msg = Position() self.cmd_pos_msg.header.seq = 1 self.cmd_pos_msg.header.stamp = rospy.Time.now() self.cmd_pos_msg.header.frame_id = self.world_frame self.cmd_pos_msg.yaw = 0 self.cmd_stop_pub = rospy.Publisher(self.cf_id + "/cmd_stop", Empty_msg, queue_size=1) self.cmd_stop_msg = Empty_msg() self.current_state_pub = rospy.Publisher(self.cf_id + "/state", String, queue_size=1)
def cfposition_from_pose(pose): roll, pitch, yaw = euler_from_quaternion( (pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w)) position = Position(pose.header, pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, yaw * 180.0 / math.pi) return position
def handle_spin(empty): rospy.loginfo('Spin server called') state = True angle = 0 pub_cmd = rospy.Publisher('hover', Position, queue_size=2) msg=rospy.wait_for_message('/cf1/pose',PoseStamped) # rospy.loginfo(msg) x = msg.pose.position.x y = msg.pose.position.y z = msg.pose.position.z roll, pitch, yaw = euler_from_quaternion((msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w)) cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = msg.header.frame_id cmd.x = x cmd.y = y cmd.z = z cmd.yaw = yaw endYaw=yaw + 360 ## STAND ALONE # while count<50: # pub_cmd.publish(cmd) # rospy.sleep(0.1) # count += 1 rospy.sleep(0.4) r = rospy.Rate(5) while(state==True): print(cmd.yaw) angle = 4 cmd.yaw = cmd.yaw + angle if cmd.yaw >= endYaw: cmd.yaw = endYaw - 360 state = False r.sleep() pub_cmd.publish(cmd) rospy.loginfo('Spin service done!') return EmptyResponse()
def __init__(self, id, initialPosition, timeHelper): self.id = id self.prefix = "/cf" + str(id) self.initialPosition = np.array(initialPosition) self.timeHelper = timeHelper # not used here, just compatible with crazyflieSim self.tf = tf.TransformListener() self.my_frame = "vicon" + self.prefix + self.prefix rospy.wait_for_service(self.prefix + "/set_group_mask") self.setGroupMaskService = rospy.ServiceProxy( self.prefix + "/set_group_mask", SetGroupMask) rospy.wait_for_service(self.prefix + "/takeoff") self.takeoffService = rospy.ServiceProxy(self.prefix + "/takeoff", Takeoff) rospy.wait_for_service(self.prefix + "/land") self.landService = rospy.ServiceProxy(self.prefix + "/land", Land) rospy.wait_for_service(self.prefix + "/stop") self.stopService = rospy.ServiceProxy(self.prefix + "/stop", Stop) rospy.wait_for_service(self.prefix + "/go_to") self.goToService = rospy.ServiceProxy(self.prefix + "/go_to", GoTo) rospy.wait_for_service(self.prefix + "/upload_trajectory") self.uploadTrajectoryService = rospy.ServiceProxy( self.prefix + "/upload_trajectory", UploadTrajectory) rospy.wait_for_service(self.prefix + "/start_trajectory") self.startTrajectoryService = rospy.ServiceProxy( self.prefix + "/start_trajectory", StartTrajectory) rospy.wait_for_service(self.prefix + "/update_params") self.updateParamsService = rospy.ServiceProxy( self.prefix + "/update_params", UpdateParams) self.cmdFullStatePublisher = rospy.Publisher(self.prefix + "/cmd_full_state", FullState, queue_size=1) self.cmdFullStateMsg = FullState() self.cmdFullStateMsg.header.seq = 0 self.cmdFullStateMsg.header.frame_id = "/world" self.cmdPositionPublisher = rospy.Publisher(self.prefix + "/cmd_position", Position, queue_size=1) self.cmdPositionMsg = Position() self.cmdPositionMsg.header.seq = 0 self.cmdPositionMsg.header.frame_id = "/world" self.cmdHoverPublisher = rospy.Publisher(self.prefix + "/cmd_hover", Hover, queue_size=1) self.cmdHoverMsg = Hover() self.cmdHoverMsg.header.seq = 0 self.cmdHoverMsg.header.frame_id = "/world" self.cmdStopPublisher = rospy.Publisher(self.prefix + "/cmd_stop", std_msgs.msg.Empty, queue_size=1)
def __init__(self, id, initialPosition, tf): """Constructor. Args: id (int): Integer ID in range [0, 255]. The last byte of the robot's radio address. initialPosition (iterable of float): Initial position of the robot: [x, y, z]. Typically on the floor of the experiment space with z == 0.0. tf (tf.TransformListener): ROS TransformListener used to query the robot's current state. """ self.id = id prefix = "/cf" + str(id) self.prefix = prefix self.initialPosition = np.array(initialPosition) self.tf = tf rospy.wait_for_service(prefix + "/set_group_mask") self.setGroupMaskService = rospy.ServiceProxy(prefix + "/set_group_mask", SetGroupMask) rospy.wait_for_service(prefix + "/takeoff") self.takeoffService = rospy.ServiceProxy(prefix + "/takeoff", Takeoff) rospy.wait_for_service(prefix + "/land") self.landService = rospy.ServiceProxy(prefix + "/land", Land) #rospy.wait_for_service(prefix + "/stop") #self.stopService = rospy.ServiceProxy(prefix + "/stop", Stop) rospy.wait_for_service(prefix + "/go_to") self.goToService = rospy.ServiceProxy(prefix + "/go_to", GoTo) rospy.wait_for_service(prefix + "/upload_trajectory") self.uploadTrajectoryService = rospy.ServiceProxy(prefix + "/upload_trajectory", UploadTrajectory) rospy.wait_for_service(prefix + "/start_trajectory") self.startTrajectoryService = rospy.ServiceProxy(prefix + "/start_trajectory", StartTrajectory) rospy.wait_for_service(prefix + "/notify_setpoints_stop") self.notifySetpointsStopService = rospy.ServiceProxy(prefix + "/notify_setpoints_stop", NotifySetpointsStop) rospy.wait_for_service(prefix + "/update_params") self.updateParamsService = rospy.ServiceProxy(prefix + "/update_params", UpdateParams) self.cmdFullStatePublisher = rospy.Publisher(prefix + "/cmd_full_state", FullState, queue_size=1) self.cmdFullStateMsg = FullState() self.cmdFullStateMsg.header.seq = 0 self.cmdFullStateMsg.header.frame_id = "/world" self.cmdStopPublisher = rospy.Publisher(prefix + "/cmd_stop", std_msgs.msg.Empty, queue_size=1) self.cmdVelPublisher = rospy.Publisher(prefix + "/cmd_vel", geometry_msgs.msg.Twist, queue_size=1) self.cmdPositionPublisher = rospy.Publisher(prefix + "/cmd_position", Position, queue_size=1) self.cmdPositionMsg = Position() self.cmdPositionMsg.header.seq = 0 self.cmdPositionMsg.header.frame_id = "/world" self.cmdVelocityWorldPublisher = rospy.Publisher(prefix + "/cmd_velocity_world", VelocityWorld, queue_size=1) self.cmdVelocityWorldMsg = VelocityWorld() self.cmdVelocityWorldMsg.header.seq = 0 self.cmdVelocityWorldMsg.header.frame_id = "/world"
def __init__(self): self.curr_pos = rospy.Subscriber('/cf1/pose', PoseStamped, self.position_callback) self.cmd_pos = rospy.Subscriber('/cf1/cmd_position', Position, self.cmd_callback) self.termin = False self.cmd_position = Position() self.current_position = PoseStamped() self.dist = 1 super(gotopos, self).__init__('gotopos')
def __init__(self): self.started = False self.pos_setpoint = [0.0, 0.0, 0.0, 0.0] rospy.init_node("send_pos_setpoints", anonymous=True) self.worldFrame = rospy.get_param("~worldFrame", "/world") self.msg = Position() self.pub = rospy.Publisher("cmd_position", Position, queue_size=1) self.sub = rospy.Subscriber("pos_from_terminal", Twist, self.getPose) self.rate = rospy.Rate(10)
def publish_hover(goal): tmp = PoseStamped() tmp.header.stamp = rospy.Time.now() tmp.header.frame_id = "map" tmp.pose.position.x = goal[0] tmp.pose.position.y = goal[1] tmp.pose.position.z = goal[2] [tmp.pose.orientation.x, tmp.pose.orientation.y, tmp.pose.orientation.z, tmp.pose.orientation.w] = quaternion_from_euler(0,0,math.radians(GOAL_YAW)) print(GOAL_YAW) if not tf_buf.can_transform(tmp.header.frame_id, 'cf1/odom', tmp.header.stamp, rospy.Duration(1) ): rospy.logwarn_throttle(2.0, 'No transform from %s to cf1/odom' % tmp.header.frame_id) return goal_odom = tf_buf.transform(tmp, 'cf1/odom', rospy.Duration(1) ) cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = "cf1/odom" cmd.x = goal_odom.pose.position.x cmd.y = goal_odom.pose.position.y cmd.z = goal_odom.pose.position.z R,P,yaw = euler_from_quaternion((goal_odom.pose.orientation.x, goal_odom.pose.orientation.y, goal_odom.pose.orientation.z, goal_odom.pose.orientation.w)) cmd.yaw = math.degrees(yaw) print(math.degrees(R)) print(math.degrees(P)) print(math.degrees(yaw)) pub_hover.publish(cmd)
def arucopose(data): for elm in data.markers: cam_aruco = PoseStamped() cam_aruco.pose = elm.pose.pose cam_aruco.header.frame_id = 'cf1/camera_link' cam_aruco.header.stamp = rospy.Time.now() print('here we are') if not tfBuffer.can_transform(cam_aruco.header.frame_id, 'cf1/odom', rospy.Time(0)): rospy.logwarn_throttle( 5.0, 'No transform from %s to cf1/odom' % cam_aruco.header.frame_id) return print('here we are (after TF)') send_transform = tfBuffer.transform(cam_aruco, 'cf1/odom', rospy.Duration(0.5)) t = TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = 'cf1/odom' t.child_frame_id = 'arucopostion' + str(elm.id) t.transform.translation = send_transform.pose.position t.transform.rotation = send_transform.pose.orientation br.sendTransform(t) ## Send position of aruco to RQT tree quaternion = quaternion_from_euler(-1.54, 0, -1.54) aruco_pos = PoseStamped() aruco_pos.pose.position.y = 0.5 aruco_pos.pose.orientation.x = quaternion[0] aruco_pos.pose.orientation.y = quaternion[1] aruco_pos.pose.orientation.z = quaternion[2] aruco_pos.pose.orientation.w = quaternion[3] aruco_pos.header.frame_id = 'arucopostion' + str(elm.id) aruco_pos.header.stamp = rospy.Time() aruco_pos.header.stamp = rospy.Time() Pose_transform = tfBuffer.transform(aruco_pos, 'cf1/odom', rospy.Duration(0.5)) _, _, yaw_transformed = euler_from_quaternion( (Pose_transform.pose.orientation.x, Pose_transform.pose.orientation.y, Pose_transform.pose.orientation.z, Pose_transform.pose.orientation.w)) yaw_transformed = round((180. / np.pi) * yaw_transformed) aruco_position = Position() aruco_position.x = Pose_transform.pose.position.x aruco_position.y = Pose_transform.pose.position.y aruco_position.z = Pose_transform.pose.position.z aruco_position.yaw = yaw_transformed aruco_position_pub.publish(aruco_position)
def publish_cmd(goal): # Need to tell TF that the goal was just generated goal.header.stamp = rospy.Time.now() if not tf_buf.can_transform(goal.header.frame_id, 'cf1/odom', goal.header.stamp): rospy.logwarn_throttle( 5.0, 'No transform from %s to cf1/odom' % goal.header.frame_id) return goal_odom = tf_buf.transform(goal, 'cf1/odom') cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = goal_odom.header.frame_id cmd.x = goal_odom.pose.position.x cmd.y = goal_odom.pose.position.y cmd.z = goal_odom.pose.position.z roll, pitch, yaw = euler_from_quaternion( (goal_odom.pose.orientation.x, goal_odom.pose.orientation.y, goal_odom.pose.orientation.z, goal_odom.pose.orientation.w)) cmd.yaw = math.degrees(yaw) pub_cmd.publish(cmd)
def pub_map_pos(goal): odom_pos = goal odom_pos.header.stamp = rospy.Time.now() if (odom_pos.header.frame_id != ''): if not buff.can_transform(odom_pos.header.frame_id, 'map', odom_pos.header.stamp, rospy.Duration(0.5)): rospy.logwarn_throttle( 5.0, 'No tranform from %s to map' % odom_pos.header.frame_id) return map_pos = buff.transform(odom_pos, 'map') # print(map_pos) cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = map_pos.header.frame_id cmd.x = map_pos.pose.position.x cmd.y = map_pos.pose.position.y cmd.z = map_pos.pose.position.z _, _, yaw = euler_from_quaternion( (map_pos.pose.orientation.x, map_pos.pose.orientation.y, map_pos.pose.orientation.z, map_pos.pose.orientation.w)) cmd.yaw = math.degrees(yaw) drone_pos_map.publish(cmd)
def onepointpublish(x, y, yaw1): global odom rate = rospy.Rate(10) goal = PoseStamped() goal.header.stamp = stamp goal.header.frame_id = "map" goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = 0.4 if not tf_buf.can_transform( 'map', 'cf1/odom', rospy.Time.now(), timeout=rospy.Duration(0.2)): rospy.logwarn_throttle( 10.0, 'No transform from %s to cf1/odom' % goal.header.frame_id) return goal_odom = tf_buf.transform(goal, 'cf1/odom') if goal_odom: #print(goal_odom.header.stamp) cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.x = goal_odom.pose.position.x cmd.y = goal_odom.pose.position.y cmd.z = goal_odom.pose.position.z cmd.yaw = yaw1 odom = cmd return odom
def positionAruco(): global tf_buff, aruco_detected trans = TransformStamped() try: # We want to keep the relative position.... We can calculate the error betwen these frames. trans = tf_buff.lookup_transform('cf1/odom', 'goal_pos', rospy.Time(0), rospy.Duration(0.5)) except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): pass # We should now have the translation and orientation of the aruco marker - the offset in x in odom frame... lets move now. cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = trans.header.frame_id cmd.x = trans.transform.translation.x cmd.y = trans.transform.translation.y cmd.z = trans.transform.translation.z roll, pitch, yaw = euler_from_quaternion( (trans.transform.rotation.x, trans.transform.rotation.y, trans.transform.rotation.z, trans.transform.rotation.w)) cmd.yaw = math.degrees(yaw) # We want to rotate opposite to the direction that the aruco rotated, else we would mirror the yaw. We should then be able to translate. pub_cmd.publish(cmd)
def publish_trajectories(self): """Publish computed trajectory """ # Set swarm manager to follow trajectory rospy.loginfo("Planner: Publishing trajectories...") time_step = 0 max_time_step = self.agents_dict[self.agents_dict.keys() [0]]['agent'].final_traj.shape[1] while time_step < max_time_step: if rospy.is_shutdown(): break for _, agent_dict in self.agents_dict.items(): agent_pos = agent_dict["agent"].final_traj[:, time_step] goal_msg = Position() goal_msg.x = agent_pos[0] goal_msg.y = agent_pos[1] goal_msg.z = agent_pos[2] goal_msg.yaw = agent_dict["start_yaw"] agent_dict['trajectory_pub'].publish(goal_msg) time_step += 1 self._rate.sleep() rospy.loginfo("Planner: Trajectory completed") self.traj_done()
def formation_demo(n_agents, formation_type): """Formation demo """ formation = get_formation(formation_type) formation_start_pos = YAML_CONF['formation']['formation_start_pos'] formation_goal = Position() formation_goal.x = formation_start_pos[0] formation_goal.y = formation_start_pos[1] formation_goal.z = formation_start_pos[2] formation_goal.yaw = formation_start_pos[3] formation.set_n_agents(n_agents) formation.set_scale(SCALE) formation.compute_formation_positions() formation.update_agents_positions(formation_goal) start_positions = read_start_positions(n_agents) goals = {ag_id: [goal.x, goal.y, goal.z] for ag_id, goal in formation._agents_goals.items()} agent_list = [] match_positions = link_agents(start_positions, goals) for each_match in match_positions: agent = Agent(AGENT_ARGS, start_pos=each_match[0], goal=each_match[1]) agent_list.append(agent) return agent_list
def main(): #global drone_pos setpoint = PoseStamped() fram = TransformStamped() setpoint.header.frame_id = 'map' setpoint.header.stamp = rospy.Time.now() print(drone_pos) setpoint.pose.position.x = drone_pos.pose.position.x #-0.25 setpoint.pose.position.y = drone_pos.pose.position.y #-0.25 setpoint.pose.position.z = drone_pos.pose.position.z #0.3 setpoint.pose.orientation.x = drone_pos.pose.orientation.x #0 setpoint.pose.orientation.y = drone_pos.pose.orientation.y #0 setpoint.pose.orientation.z = drone_pos.pose.orientation.z setpoint.pose.orientation.w = drone_pos.pose.orientation.w cmd = Position() rate = rospy.Rate(20) # Hz while not rospy.is_shutdown(): print('Point in map\n') #print(setpoint) print('============================') if not buff.can_transform('map', 'cf1/odom', rospy.Time.now(), rospy.Duration(0.5)): rospy.logwarn_throttle(5.0, 'No tranform from %s to map' % 'odom') return else: t = buff.transform(setpoint, 'cf1/odom') fram.header.stamp = rospy.Time.now() fram.header.frame_id = 'cf1/odom' fram.child_frame_id = 'goal' fram.transform.translation.x = drone_pos.pose.position.x fram.transform.translation.y = drone_pos.pose.position.y fram.transform.translation.z = drone_pos.pose.position.z fram.transform.rotation.x = drone_pos.pose.orientation.x fram.transform.rotation.y = drone_pos.pose.orientation.y fram.transform.rotation.z = drone_pos.pose.orientation.z fram.transform.rotation.w = drone_pos.pose.orientation.w tf_bc.sendTransform(fram) cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = fram.header.frame_id cmd.x = drone_pos.pose.position.x cmd.y = drone_pos.pose.position.y cmd.z = 0.3 _, _, yaw = euler_from_quaternion( (drone_pos.pose.orientation.x, drone_pos.pose.orientation.y, drone_pos.pose.orientation.z, drone_pos.pose.orientation.w)) cmd.yaw = math.degrees(yaw) pub_cmd.publish(cmd) print('Point in odom\n') print(fram) print('============================') rate.sleep()
def publish_path(): # pathx = [74.0, 76.0, 78.0, 80.0, 82.0, 84.0, 86.0, 88.0, 90.0, 92.0, 94.0, 96.0, 98.0, 100.0, 102.0, 104.0, 106.0, 108.0, 110.0, 112.0, 114.0, 116.0, 118.0, 120.0] # pathy = [100.0, 100.0, 100.0, 102.0, 102.0, 104.0, 106.0, 106.0, 106.0, 108.0, 108.0, 110.0, 110.0, 110.0, 112.0, 112.0, 114.0, 114.0, 116.0, 116.0, 118.0, 118.0, 118.0, 120.0] rospy.init_node('path_following', anonymous=True) rx, ry = find_path() # print(rx) # print(ry) pathx = [c / 100 for c in rx] pathy = [c / 100 for c in ry] pathz = [ 0.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9 ] pathyaw = [0] * 20 cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = '10' # a simple random number pub = rospy.Publisher('/cf1/cmd_position', Position, queue_size=2) rate = rospy.Rate(1) # 10hz while not rospy.is_shutdown(): for i in range(20): cmd.x = pathx[i] cmd.y = pathy[i] cmd.z = pathz[i] cmd.yaw = pathyaw[i] pub.publish(cmd) rate.sleep() for j in range(20): cmd.x = pathx[19 - j] cmd.y = pathy[19 - j] cmd.z = pathz[19 - j] cmd.yaw = pathyaw[19 - j] pub.publish(cmd) rate.sleep()
def __init__(self): self.map_pos=rospy.Subscriber('/cf1/pose_map', Position, self.callback) self.pub_hold_pos = rospy.Publisher('/cf1/hold_pos', PoseStamped, queue_size=2) rospy.loginfo("ppClient waiting for service") rospy.wait_for_service('path_planning') self.proxy = rospy.ServiceProxy('path_planning', pathPlanning) rospy.loginfo("ppClient finished waiting for service") self.signs = { 1: 'airport', 2: 'dangerous_curve_left', 3: 'dangerous_curve_right', 4: 'follow_left', 5: 'follow_right', 6: 'junction', 7: 'no_bicycle', 8: 'no_heavy_truck', 9: 'no_parking', 10: 'no_stopping_and_parking', 11: 'residential', 12: 'road_narrows_from_left', 13: 'road_narrows_from_right', 14: 'roundabout_warning', 15: 'stop' } self.cmd = Position() self.termin=False self.plan=False self.callback_called = False super(Planner, self).__init__("Planner!")
def pub(self, goal): cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = goal.header.frame_id cmd.x = goal.pose.position.x cmd.y = goal.pose.position.y cmd.z = goal.pose.position.z _, _, yaw = euler_from_quaternion( (goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z, goal.pose.orientation.w)) cmd.yaw = math.degrees(yaw) self.hold_pose.publish(cmd)
def compute_formation_positions(self): for i in range(self._n_agents): if rospy.is_shutdown(): break # Initialize agent formation goal self._agents_goals[i] = Position() # Compute formation position # TODO: Compute agent position from center # Compute information from center center_dist, theta, center_height = compute_info_from_center( [x_dist, y_dist, z_dist]) self._center_dist[i] = center_dist self._angle[i] = theta self._center_height[i] = center_height return self._agents_goals
def __init__(self): # Pull ROS parameters from launch file: param = rospy.search_param("goal_topic") self.goal_topic = rospy.get_param(param) # self.goal_topic = goal param = rospy.search_param("goal_frame") self.goal_frame = rospy.get_param(param) param = rospy.search_param("goal_pose") self.goal_pose_string = rospy.get_param(param) self.goal_pose_list = list(self.goal_pose_string.split(", ")) self.goal_pose = [float(i) for i in self.goal_pose_list] # Initialize goal message and publisher self.goal = Position() self.goal.header.frame_id = self.goal_frame self.pub_goal = rospy.Publisher(self.goal_topic, Position, queue_size=10) # Delay briefly for publisher to initialize rospy.sleep(.3)
def msg_def_crazyflie(pose, yaw): worldFrame = rospy.get_param("~worldFrame", "/world") msg = Position() msg.header.seq = 0 msg.header.stamp = rospy.Time.now() msg.header.frame_id = worldFrame msg.x = pose[0] msg.y = pose[1] msg.z = pose[2] msg.yaw = yaw now = rospy.get_time() msg.header.seq = 0 msg.header.stamp = rospy.Time.now() return msg
#!/usr/bin/env python import rospy import tf from crazyflie_driver.msg import Position from std_msgs.msg import Empty from crazyflie_driver.srv import UpdateParams if __name__ == '__main__': rospy.init_node('position', anonymous=True) worldFrame = rospy.get_param("~worldFrame", "/world") rate = rospy.Rate(10) # 10 hz name = "cmd_position" msg = Position() msg.header.seq = 0 msg.header.stamp = rospy.Time.now() msg.header.frame_id = worldFrame msg.x = 0.0 msg.y = 0.0 msg.z = 0.0 msg.yaw = 0.0 pub = rospy.Publisher(name, Position, queue_size=1) stop_pub = rospy.Publisher("cmd_stop", Empty, queue_size=1) stop_msg = Empty() rospy.wait_for_service('update_params') rospy.loginfo("found update_params service")