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")