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 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 cmd_func(self, msg): global detected, count, state, cur_x, cur_y, cur_z, cur_yaw self.msg = msg if count < 1: cur_x = self.msg.pose.position.x cur_y = self.msg.pose.position.y cur_z = self.msg.pose.position.z cur_yaw = 0 count = 1 z_dist = 0.4 diff_z = abs(msg.pose.position.z - z_dist) cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = msg.header.frame_id if state == 0: cmd.x = cur_x cmd.y = cur_y cmd.z = z_dist cmd.yaw = cur_yaw pub_cmd.publish(cmd) rospy.sleep(0.2) if diff_z < 0.05: rospy.sleep(0.2) cmd.z = cur_z state = 1 if state == 1: if detected == False: cmd.x = cur_x cmd.y = cur_y cmd.z = cur_z cmd.yaw = cur_yaw pub_cmd.publish(cmd) rospy.sleep(0.2) else: cmd.x = x cmd.y = y cmd.z = z cmd.yaw = yaw pub_cmd.publish(cmd) detected = False rospy.sleep(0.2)
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 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 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 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 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 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 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 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 __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 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 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
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 onepointpublish(x, y, yaw1): global odom #rate = rospy.Rate(10) goal = PoseStamped() goal.header.stamp = rospy.Time.now() goal.header.frame_id = "map" goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = 0.4 goal.pose.orientation.x = 0.0 goal.pose.orientation.y = 0.0 goal.pose.orientation.z = -0.999021480034635 goal.pose.orientation.w = -0.0442276206618389 # roll, pitch, yaw = euler_from_quaternion((goal.pose.orientation.x, # goal.pose.orientation.y, # goal.pose.orientation.z, # goal.pose.orientation.w)) #print(goal.header.stamp) if not tf_buf.can_transform(goal.header.frame_id, '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() #print(goal_odom.header.frame_id) odom cmd.x = goal_odom.pose.position.x cmd.y = goal_odom.pose.position.y cmd.z = goal_odom.pose.position.z #print(cmd.x,cmd.y,cmd.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) # yaw is directly in odom frame cmd.yaw = yaw1 odom = cmd
def _go_to_srv(self, srv_req): goals = ast.literal_eval(srv_req.goals) target_goals = {} formation_goal = None # Make sure swarm is in hover or formation state if self._state_machine.in_state( "in_formation") or self._state_machine.in_state("hover"): for goal_id, goal_val in goals.items(): if goal_id == "formation": # print "Formation goal: {}".format(goal_val) formation_goal = goal_val elif goal_id in self._cf_list: # print "{} goal: {}".format(goal_id, goal_val) new_goal = Position() new_goal.x = goal_val[0] new_goal.y = goal_val[1] new_goal.z = goal_val[2] new_goal.yaw = goal_val[3] target_goals[goal_id] = new_goal else: rospy.logerr("%s: Invalid goal name" % goal_id) if self.ctrl_mode == "automatic": self._after_traj_state = "hover" self.go_to_goal(target_goals=target_goals) elif self.ctrl_mode == "formation": self._after_traj_state = "in_formation" self.update_formation(formation_goal=formation_goal) else: rospy.logerr("Swarm not ready to move") return {}
def map_to_odom(x, y): #rate = rospy.Rate(10) goal = PoseStamped() goal.header.stamp = rospy.Time.now() goal.header.frame_id = 'map' goal.pose.position.x = x goal.pose.position.y = y goal.pose.position.z = 0.4 #goal.pose.orientation.z = 0 # if not tf_buf.can_transform(goal.header.frame_id, 'cf1/odom', goal.header.stamp): # rospy.logwarn_throttle(10.0, 'No transform from %s to cf1/odom' % goal.header.frame_id) # return if not tf_buf.can_transform(goal.header.frame_id, 'cf1/odom', rospy.Time.now(), timeout=rospy.Duration(3)): rospy.logwarn_throttle( 10.0, 'No transform from %s to cf1/odom' % goal.header.frame_id) return #trans = tf_buf.lookup_transform('cf1/odom',"map",rospy.Time(0),rospy.Duration(0.4)) goal_odom = tf_buf.transform(goal, 'cf1/odom') cmd = Position() cmd.header.stamp = rospy.Time.now() #print(trans.header.frame_id) cmd.x = goal_odom.pose.position.x cmd.y = goal_odom.pose.position.y cmd.z = goal_odom.pose.position.z #print(cmd.x,cmd.y,cmd.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) map_to_odom = cmd return map_to_odom
def pub(goal): pub_ = goal pub_.header.stamp = rospy.Time.now() if not buff.can_transform(pub_.header.frame_id, 'cf1/odom', pub_.header.stamp, rospy.Duration(0.5)): rospy.logwarn_throttle(5.0 , 'No tranform from %s to cf1/odom' % pub_.header.frame_id) return odo = buff.transform(pub_ , 'cf1/odom') cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = odo.header.frame_id cmd.x = odo.pose.position.x cmd.y = odo.pose.position.y cmd.z = odo.pose.position.z _, _, yaw = euler_from_quaternion((odo.pose.orientation.x, odo.pose.orientation.y, odo.pose.orientation.z, odo.pose.orientation.w)) cmd.yaw = math.degrees(yaw) pub_cmd.publish(cmd)
def callback(empty): rospy.sleep(1) pub_cmd = rospy.Publisher('hover', Position, queue_size=2) msg = rospy.wait_for_message('/cf1/pose', PoseStamped) 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 = msg.pose.position.x cmd.y = msg.pose.position.y cmd.yaw = yaw cmd.z = 0.4 rospy.loginfo('Takeoff service called') pub_cmd.publish(cmd) rospy.loginfo('Service done!') return EmptyResponse()
def pub(goal): global cmd_pos pub_ = goal pub_.header.stamp = rospy.Time.now() print("Before transformation\n") print(pub_) print("=============================\n") if (pub_.header.frame_id != ''): # pub_.header.frame_id='map' print('Before sending: ') if (pub_.header.frame_id != 'cf1/odom'): if not buff.can_transform(pub_.header.frame_id, 'cf1/odom', pub_.header.stamp, rospy.Duration(0.5)): rospy.logwarn_throttle( 5.0, 'No tranform from %s to cf1/odom' % pub_.header.frame_id) return odo = buff.transform(pub_, 'cf1/odom') else: odo = pub_ # print(odo) cmd = Position() cmd.header.stamp = rospy.Time.now() cmd.header.frame_id = odo.header.frame_id cmd.x = odo.pose.position.x cmd.y = odo.pose.position.y cmd.z = 0.35 _, _, yaw = euler_from_quaternion( (odo.pose.orientation.x, odo.pose.orientation.y, odo.pose.orientation.z, odo.pose.orientation.w)) cmd.yaw = math.degrees(yaw) print('Before sending: ') print(cmd) pub_cmd.publish(cmd)
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) start_x = 120.0 # [m] start_y = 100.0 # [m] end_x = 20.0 # [m] end_y = 100.0 # [m] # rx,ry = find_path(start_x,start_y,end_x,end_y) path = path_planning_client(start_x, start_y, end_x, end_y) # request message print(path) # rx:[....]\n ry:[....] # print(path.rx) # (xx, xx, xx) # print(type(path.rx)) #<class 'tuple'> # for i in range(5): # print((path.rx[i])) # print(rx) # print(ry) pathx = [c * 0.05 for c in path.rx] # tranfer from pixels to meters pathy = [c * 0.05 for c in path.ry] pathz = [0.6] * len(pathx) pathyaw = [0] * len(pathx) 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(len(pathx)): cmd.x = pathx[i] cmd.y = pathy[i] cmd.z = pathz[i] cmd.yaw = pathyaw[i] pub.publish(cmd) rate.sleep()
except: print('Caught exception looking up map -> cf1/base_link tf') init_tf = TransformStamped() init_tf.transform.translation.x = 0.5 init_tf.transform.translation.y = 0.5 init_tf.transform.rotation.w = 1 # init_tf.transform.translation.z = 0.5 goal = Position() goal.x = init_tf.transform.translation.x goal.y = init_tf.transform.translation.y goal.z = 0.5 init_quat = (init_tf.transform.rotation.x, init_tf.transform.rotation.y, init_tf.transform.rotation.z, init_tf.transform.rotation.w) _, _, yaw = euler_from_quaternion(init_quat) goal.yaw = math.degrees(yaw) print('Initial goal = starting position') print(goal) rospy.sleep(3) rate = rospy.Rate(10) while not rospy.is_shutdown(): if goal: try: # Map in odom coords (order very important!!) odom_tf = tf_buffer.lookup_transform("cf1/odom", "map", rospy.Time.now(), rospy.Duration(0.5)) except:
# INitialize global variable for actual position of crazyflie current_position = Position() # message used to store pos info to send next_pos = Position() # Choose a trajectory chosen_traj = raw_input('[ circle spiral conical_helix uniform_helix ]') # Takeoff first -> go to 0, 0 , 1 while ((0 - current_position.x)**2 + (0 - current_position.y)**2 + (1 - current_position.z)**2) > 1e-2: # 10cm next_pos.x = 0 next_pos.y = 0 next_pos.z = 1 next_pos.yaw = 0.0 next_pos.header.seq += 1 next_pos.header.stamp = rospy.Time.now() pubSetpointPos.publish(next_pos) rate.sleep() t = list() x = list() y = list() z = list() if chosen_traj == 'circle': (t, x, y) = circle_trajectory(freq_pub, 15, 1) z = [z_plan for val in range(len(x))] elif chosen_traj == 'spiral': (t, x, y) = fermat_spiral(freq_pub, 15) z = [z_plan for val in range(len(x))]
from crazyflie_driver.srv import UpdateParams if __name__ == '__main__': rospy.init_node('qualisys_hover', anonymous=True) worldFrame = rospy.get_param("~worldFrame", "/world") rate = rospy.Rate(10) # 10 hz 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("cmd_position", 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") update_params = rospy.ServiceProxy('update_params', UpdateParams) rospy.set_param("stabilizer/estimator", 2) update_params(["stabilizer/estimator"]) rospy.set_param("kalman/resetEstimation", 1) update_params(["kalman/resetEstimation"]) rospy.sleep(0.1)
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") update_params = rospy.ServiceProxy('update_params', UpdateParams) rospy.set_param("kalman/resetEstimation", 1) update_params(["kalman/resetEstimation"]) rospy.sleep(0.1) rospy.set_param("kalman/resetEstimation", 0) update_params(["kalman/resetEstimation"])
def goal_callback(msg): print("Have goal") global goal # Keeps distance to aruco goal=msg # Initializing Position and goal Current_Position=Position() Current_Position.x=0.5 #in real environment Current_Position.y=0.5 Current_Position.z=0.4 Current_Position.yaw=0 goal=None if __name__ == '__main__': rospy.init_node('arucorelativepose') aruco_sub=rospy.Subscriber("aruco_position_pose", Position, goal_callback) # Goal position is the aruco position? pos_publish=rospy.Publisher("/cf1/cmd_position", Position,queue_size=10) rate = rospy.Rate(10) while not rospy.is_shutdown(): if goal: print("goal")
def obstacle_sequence(self, sign_class): rospy.loginfo('Begin sequence for next road sign') print('Sign class: ', sign_class) rospy.sleep(3) """ Calc observation pose based on sign pose""" offset = 0.5 # [m] | How far away to look at sign yaw = self.objects[sign_class][5] # rpy observe_pose = Position() observe_pose.x = self.objects[sign_class][0] - math.cos(math.radians(yaw))*offset observe_pose.y = self.objects[sign_class][1] - math.sin(math.radians(yaw))*offset observe_pose.z = 0.5 observe_pose.yaw = yaw """ use tf to get the starting position in the map frame """ start_pose_ = self.tf_buffer.lookup_transform("map", "cf1/base_link", rospy.Time.now(), rospy.Duration(10)) """ Define path planning start and end in pixel coords """ resolution = 0.05 start_x = int(round(-start_pose_.transform.translation.x/resolution)+100) # transfer it from meters to pixels start_y = int(round(-start_pose_.transform.translation.y/resolution)+100) end_x = int(round(-observe_pose.x/resolution)+100) end_y = int(round(-observe_pose.y/resolution)+100) """ Call path planning service """ rospy.loginfo('Starting path planning') path_planning = rospy.ServiceProxy('path_planning', PathPlanning) path = path_planning(start_x, start_y, end_x, end_y) """ Get path as """ pathx = [-(c-100)*resolution for c in path.rx] # In meters, which can be published to the /cf1/cmd_position pathy = [-(c-100)*resolution for c in path.ry] # In meters, which can be published to the /cf1/cmd_position path_yaw = [] for i in range(len(pathx)-1): yaw = math.degrees(math.atan2(pathy[i+1]-pathy[i],pathx[i+1]-pathx[i])) path_yaw.append(yaw) path_yaw.append(observe_pose.yaw) rospy.loginfo('Finished path planning') """ Build path msg and visualize in RVIZ """ path_msg = Path() path_msg.header.frame_id = 'map' poses = [] for i in range(len(pathx)): pose = PoseStamped() # pose.header.frame_id = 'map' pose.pose.position.x = pathx[i] pose.pose.position.y = pathy[i] pose.pose.position.z = 0.5 poses.append(pose) path_msg.poses = poses path_msg.header.stamp = rospy.Time.now() self.path_pub.publish(path_msg) """ Add rotation to path direction before beginning path """ """ Follow the path """ path_pose = Position() path_pose.z = 0.5 path_rate = rospy.Rate(1) for i in range(len(pathx)): path_pose.x = pathx[i] path_pose.y = pathy[i] path_pose.yaw = path_yaw[i] self.goal_pub.publish(path_pose) rospy.loginfo('Sent next path pose') # Republish path for visualization path_msg.header.stamp = rospy.Time.now() self.path_pub.publish(path_msg) path_rate.sleep() rospy.sleep(2) self.goal_pub.publish(observe_pose) rospy.loginfo('Confirm at end of path - 1x') rospy.sleep(3) self.goal_pub.publish(observe_pose) rospy.loginfo('Confirm at end of path - 2x') rospy.sleep(3) rospy.loginfo("Made it to pose for observing sign") """ Wait for detection """ self.boxes = None rate = rospy.Rate(1) rospy.loginfo("Start looking for road sign detections") saw_sign = False while not saw_sign: if self.boxes: for box in self.boxes: print('Seeing', box.Class) if box.Class == sign_class: rospy.loginfo('Confirmed detection of correct sign') saw_sign = True self.boxes = None rate.sleep() """ Call clear checkpoint """ clear_pose = GoToRequest() clear_pose.goal.x = observe_pose.x clear_pose.goal.y = observe_pose.y clear_pose.goal.z = observe_pose.z clear_pose.yaw = observe_pose.yaw try: rospy.loginfo('Starting clear checkpoint sequence') checkpoint = rospy.ServiceProxy('clearpointservice', GoTo) checkpoint(clear_pose) rospy.loginfo('Checkpoint cleared') except rospy.ServiceException as e: print ("Service call failed: %s" % e) rospy.sleep(3) """ Fix odometry """ self.goal_pub.publish(observe_pose) rospy.loginfo("Correct pose to observation pose - 1x") rospy.sleep(3) self.goal_pub.publish(observe_pose) rospy.loginfo("Correct pose to observation pose - 2x") rospy.sleep(3)
rospy.Subscriber("Ranging2", GenericLogData, callback_beacon_ranging2) rospy.Subscriber("TWRtime", GenericLogData, callback_twr_time) rospy.Subscriber("TWRbeacons", GenericLogData, callback_twr_beacon) rospy.Subscriber("TWRother", GenericLogData, callback_twr_other) rospy.Subscriber("TWReve", GenericLogData, callback_twr_eve) rospy.Subscriber("TWRenc", GenericLogData, callback_twr_enc) rospy.Subscriber("encTime", GenericLogData, callback_twr_encTime) #for position mode msgPos = Position() msgPos.header.seq = 0 msgPos.header.stamp = rospy.Time.now() msgPos.header.frame_id = worldFrame msgPos.x = 0.0 msgPos.y = 0.0 msgPos.z = 0.0 msgPos.yaw = 0.0 pubPos = rospy.Publisher("cmd_setpoint", Position, queue_size=1) #publish to console msgConsole = ConsoleMessage() msgConsole.header.seq = 0 msgConsole.header.stamp = rospy.Time.now() msgConsole.header.frame_id = worldFrame msgConsole.data = [ord('%'), ord('T'), ord('S'), 0] pubConsole = rospy.Publisher("cmd_console_msg", ConsoleMessage, queue_size=1) #msgConsole.data = [ord('%'), ord('T'), ord('S'), delta_p_param, 0, 0, 0] # CHANGE TDMA SLOT #msgConsole.data = [ord('%'), ord('O'), ord('F'), 0, 0, 0, 0] # CHANGE TO FIXED ORDER #msgConsole.data = [ord('%'), ord('O'), ord('R'), 0, 0, 0, 0] # CHANGE TO RANDOM ORDER #msgConsole.data = [ord('%'), ord('O'), ord(Order), 0, 0, 0, 0]
vy = rospy.get_param("~vy") x = rospy.get_param("~x") y = rospy.get_param("~y") z = rospy.get_param("~z") yaw = rospy.get_param("~yaw") 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 = x msg.y = y msg.z = z msg.yaw = yaw msg.vx = vx msg.vy = vy 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") update_params = rospy.ServiceProxy('update_params', UpdateParams) # go to x: 0.2 y: 0.2 start = rospy.get_time() while not rospy.is_shutdown():