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():