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 handle_spin(empty):
rospy.loginfo('Server called')
state = True
angle = 0
pub_cmd = rospy.Publisher('/cf1/cmd_position', Position, queue_size=2)
rospy.loginfo('Waiting for pose message...')
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.2)
r = rospy.Rate(10)
while (state == True):
angle = 4
cmd.yaw = cmd.yaw + angle
# rospy.loginfo(angle)
if cmd.yaw >= endYaw:
cmd.yaw = endYaw - 360
state = False
rospy.loginfo(cmd.yaw)
r.sleep()
pub_cmd.publish(cmd)
rospy.loginfo('Spin service done!')
return EmptyResponse()
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 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 _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 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):
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 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()
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 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)
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(6) # hz
name = "cmd_setpoint"
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)
def navigation(self):
mass = 0.027
dt = 0.01
mode = 0
second_reg = False
grav = 9.81
x = [
np.zeros(3),
np.zeros(3),
np.zeros(3),
np.zeros(3),
np.zeros(3),
np.zeros(3)
]
while not rospy.is_shutdown():
for i in range(0, 1):
x[i] = self.position_from_odometry(self.agent_pose[i])
# Check if we are lider o follower
if self.priority == 1: # Lider
mode = 1
else: # Follower
mode = 0
# Point to achieve
xd = self.PoI[self.region_idx]
if mode == 1:
x_desired = xd
ep = x[0] - xd
if np.linalg.norm(
ep) < 0.05: # Cheqck if desired point is achieved
if self.region_idx == 3:
print('REACHED!!! POINT NUMBER ' +
str(self.region_idx))
self.region_idx = 0
if self.region_idx < 3:
print('REACHED!!! POINT NUMBER ' +
str(self.region_idx))
self.region_idx += 1 # Another point to achieve
# Define the conections in the graph
# Edges = {(0,1), (0,2), (0,3), (2,3), (2,4), (4,5), (5,1)}
# Definition of etas depending on the Crazyflie we are
if self.agent_number == 1:
distancia_0 = x[1] - x[0]
distancia_1 = x[2] - x[0]
x_desired = x[0] + self.conections(
distancia_0) - self.collision(distancia_1)
# if self.agent_number == 2:
# distancia_0 = x[1] - x[0]
# distancia_1 = x[2] - x[0]
# x_desired = x[0] + self.conections(distancia_0) - self.collision(distancia_1)
# if self.agent_number == 3:
# eta_con[0], eta_con_dot[0] = self.eta_funtion(x[0], x[1], v[0], v[1])
# eta_con[1], eta_con_dot[1] = self.eta_funtion(x[0], x[3], v[0], v[3])
# if self.agent_number == 4:
# eta_con[0], eta_con_dot[0] = self.eta_funtion(x[0], x[3], v[0], v[3])
# eta_con[1], eta_con_dot[1] = self.eta_funtion(x[0], x[5], v[0], v[5])
# if self.agent_number == 5:
# eta_con[0], eta_con_dot[0] = self.eta_funtion(x[0], x[5], v[0], v[5])
# eta_con[1], eta_con_dot[1] = self.eta_funtion(x[0], x[2], v[0], v[2])
mesage_to_pub = Position()
mesage_to_pub.x = x_desired[0]
mesage_to_pub.y = x_desired[1]
mesage_to_pub.z = x_desired[2]
self.force_pub.publish(mesage_to_pub)
self.rate.sleep()
if __name__ == '__main__':
rospy.init_node('follower', anonymous=True)
worldFrame = rospy.get_param("~worldFrame", "/world")
#cf18 = crazyflie.Crazyflie("crazyflie18", "/vicon/crazyflie18/crazyflie18")
#cf15 = crazyflie.Crazyflie("crazyflie15", "/vicon/crazyflie15/crazyflie15")
rate = rospy.Rate(20) # 20 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
target_pub18 = rospy.Publisher("/crazyflie18/cmd_position",
Position,
queue_size=1)
target_pub15 = rospy.Publisher("/crazyflie15/cmd_position",
Position,
queue_size=1)
human_sub = rospy.Subscriber("/vicon/human/human", TransformStamped,
human_cb)
cf18_sub = rospy.Subscriber("/vicon/crazyflie18/crazyflie18",
TransformStamped, cf18_cb)
cf15_sub = rospy.Subscriber("/vicon/crazyflie15/crazyflie15",
TransformStamped, cf15_cb)
if __name__ == '__main__':
rospy.init_node('binded2', anonymous=True)
worldFrame = rospy.get_param("~worldFrame", "/world")
cf18 = crazyflie.Crazyflie("crazyflie18", "/vicon/crazyflie18/crazyflie18")
cf15 = crazyflie.Crazyflie("crazyflie15", "/vicon/crazyflie15/crazyflie15")
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
target_pub18 = rospy.Publisher("/crazyflie18/cmd_position",
Position,
queue_size=1)
target_pub15 = rospy.Publisher("/crazyflie15/cmd_position",
Position,
queue_size=1)
cf18_sub = rospy.Subscriber("/vicon/crazyflie18/crazyflie18",
TransformStamped, cf18_cb)
cf15_sub = rospy.Subscriber("/vicon/crazyflie15/crazyflie15",
TransformStamped, cf15_cb)
joy_sub = rospy.Subscriber("/joy", Joy, joy_cb)
from crazyflie_driver.msg import Position
from std_msgs.msg import Empty
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)
from crazyflie_driver.msg import Position
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():
try:
init_tf = tf_buffer.lookup_transform("map", "cf1/base_link",
rospy.Time.now(),
rospy.Duration(10))
# init_tf.transform.translation.z = 0.5
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:
if __name__ == '__main__':
rospy.init_node('trajectory', anonymous=True)
worldFrame = rospy.get_param("~worldFrame", "/world")
trajectory0 = np.array(pd.read_csv('trajectory0.csv'))
trajectory1 = np.array(pd.read_csv('trajectory1.csv'))
trajectory2 = np.array(pd.read_csv('trajectory2.csv'))
cf18 = crazyflie.Crazyflie("crazyflie18", "/vicon/crazyflie18/crazyflie18")
cf15 = crazyflie.Crazyflie("crazyflie15", "/vicon/crazyflie15/crazyflie15")
cf13 = crazyflie.Crazyflie("crazyflie13", "/vicon/crazyflie13/crazyflie13")
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
t1 = Thread(target=handler, args=(cf18, trajectory0))
t2 = Thread(target=handler, args=(cf15, trajectory1))
t3 = Thread(target=handler, args=(cf13, trajectory2))
t1.start()
t2.start()
t3.start()
def cmd_func(msg):
global state, angle, count, count_pose, Xm, N
if count_pose < 1:
init_pose(msg)
count_pose = 1
z_dist = 0.5
diff_z = abs(msg.pose.position.z - z_dist)
diff_x = abs(msg.pose.position.x - Xm)
cmd = Position()
cmd.header.stamp = rospy.Time.now()
cmd.header.frame_id = msg.header.frame_id
##############################################################
if state == 0:
cmd.x = x_init
cmd.y = y_init
cmd.z = z_dist
cmd.yaw = 0
pub_cmd.publish(cmd)
if diff_z < 0.05:
rospy.sleep(0.2)
state = 1
##############################################################
if state == 1:
cmd.x = x_init + Xm
cmd.y = y_init
cmd.z = z_dist
cmd.yaw = 0
pub_cmd.publish(cmd)
if diff_x < 0.02:
rospy.sleep(0.2)
state = 3
##################################################################
if state == 3:
cmd.x = x_init + Xm
cmd.y = y_init
cmd.z = z_dist
angle += 2
if angle == 360:
angle = 0
count += 1
rospy.loginfo("Lap: " + str(count))
cmd.yaw = angle
pub_cmd.publish(cmd)
if count == N:
if angle == 180:
state = 4
rospy.sleep(0.2)
##################################################################
if state == 4:
cmd.x = x_init
cmd.y = y_init
cmd.z = z_dist
cmd.yaw = 180
pub_cmd.publish(cmd)
start_time = rospy.Time.now()
SCALE = 0.5
traj = np.array( pd.read_csv('minjerk_trajectory.csv') ) * SCALE
if direction=='inverse':
RANGE = reversed(range(len(traj)))
else:
RANGE = range(len(traj))
for i in RANGE:
msg.header.seq += 1
msg.header.stamp = rospy.Time.now()
t = (msg.header.stamp - start_time).to_sec()
msg.x = traj[i,0]
msg.y = traj[i,1]
msg.z = traj[i,2]
msg.yaw = 0
pub.publish(msg)
print msg
rate.sleep()
# landing
rospy.loginfo('Landing...')
while not rospy.is_shutdown():
msg.z -= 0.02
if ( cf_pos.transform.translation.z < 0.11 ):
break
msg.header.seq += 1
msg.header.stamp = rospy.Time.now()
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")
update_params = rospy.ServiceProxy('update_params', UpdateParams)
rospy.set_param("kalman/resetEstimation", 1)
update_params(["kalman/resetEstimation"])
rospy.sleep(0.1)
rospy.Subscriber("KF", GenericLogData, callback_pos_beacons2)
rospy.Subscriber("Ranging1", GenericLogData, callback_beacon_ranging1)
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
rate = rospy.Rate(freq_pub)
# 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':
worldFrame = rospy.get_param("~worldFrame", "/world")
vx = rospy.get_param("~vx")
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
queue_size=10)
next_pos = Position()
current_position = Position()
#vel_msg = Twist()
#vel_msg.linear.x=1.
#vel_msg.linear.y=0.
#vel_msg.linear.z=0.
#vel_msg.angular.x = 0.
#vel_msg.angular.y = 0.
#vel_msg.angular.z = 0.
a = 0
while a < 1e5:
next_pos.x = 0.5 / freq_pub + current_position.x
next_pos.y = 0. / freq_pub + current_position.y
next_pos.z = 0. / freq_pub + current_position.z
next_pos.header.seq += 1
next_pos.header.stamp = rospy.Time.now()
#pubSetpointVel.publish(vel_msg)
pubSetpointPos.publish(next_pos)
a += 1
# print('a',a)
# print('current_position=',current_position)
# print('next_pos=',next_pos)
rate.sleep()
time.sleep(3.0)
#land
cf_1.land(targetHeight=0.0, duration=2.0)
rospy.init_node('follower', anonymous=True)
worldFrame = rospy.get_param("~worldFrame", "/world")
tr = np.array(pd.read_csv('~/Desktop/crazyflies/trajectory.csv'))
cf18 = crazyflie.Crazyflie("crazyflie18", "/vicon/crazyflie18/crazyflie18")
#cf15 = crazyflie.Crazyflie("crazyflie15", "/vicon/crazyflie15/crazyflie15")
rate = rospy.Rate(5) # 20 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
target_pub18 = rospy.Publisher("/crazyflie18/cmd_position",
Position,
queue_size=1)
#target_pub15 = rospy.Publisher("/crazyflie15/cmd_position", Position, queue_size=1)
human_sub = rospy.Subscriber("/vicon/human/human", TransformStamped,
human_cb)
cf18_sub = rospy.Subscriber("/vicon/crazyflie18/crazyflie18",
TransformStamped, cf18_cb)
#cf15_sub = rospy.Subscriber("/vicon/crazyflie18/crazyflie18", TransformStamped, cf15_cb)
joy_sub = rospy.Subscriber("/joy", Joy, joy_cb)
stop_pub18 = rospy.Publisher("/crazyflie18/cmd_stop", Empty, queue_size=1)