def repel(self, cf_position, ob_position, cf_id, ob_id):
# Get x,y velocities from tangential repulsion
tan_angle = math.atan2(cf_position[1] - ob_position[1],
cf_position[0] - ob_position[0])
tan_angle = (tan_angle + 2 * math.pi) % (2 * math.pi)
vel_xy = np.array([math.cos(tan_angle), math.sin(tan_angle)])
# Convert to max velocities for avoidance
for i in range(2):
if vel_xy[i] > 0:
vel_xy[i] = 0.3
elif vel_xy[i] < 0:
vel_xy[i] = -0.3
# Get vertical avoidance
z_increase = cf_position[2] >= ob_position[2]
action_msg = Hover()
action_msg.vx = vel_xy[0]
action_msg.vy = vel_xy[1]
action_msg.zDistance = cf_position[2]
action_msg.yawrate = 0
# Num times to send the repulison Hover cmd
for _ in range(2):
cf_position = self.get_position(self.cf_id)
# Bounce drone off right wall
if cf_position[0] >= 9.5 and action_msg.vx > 0:
action_msg.vx = 0
cf_position = self.get_position(self.cf_id)
# Bounce drone off left wall
elif cf_position[0] <= -9.5 and action_msg.vx < 0:
action_msg.vx = 0
cf_position = self.get_position(self.cf_id)
# Bounce drone off back wall
if cf_position[1] >= 9.5 and action_msg.vy > 0:
action_msg.vy = 0
cf_position = self.get_position(self.cf_id)
# Bounce drone off front wall
elif cf_position[1] <= -9.5 and action_msg.vy < 0:
action_msg.vy = 0
cf_position = self.get_position(self.cf_id)
# Move cf upwards or downwards based on relative position of obstacle
if z_increase:
action_msg.zDistance += 0.1
else:
action_msg.zDistance -= 0.1
action_msg.zDistance = np.clip(action_msg.zDistance, 0.5, 9.5)
print('Repel %i from %i' % (cf_id, ob_id))
self.hover_pub.publish(action_msg)
time.sleep(0.3)
def process_action(self, action):
action[0] = self.unnormalize(action[0], -0.4, 0.4)
action[1] = self.unnormalize(action[1], -0.4, 0.4)
action[2] = self.unnormalize(action[2], 0.25, 9.75)
action[3] = self.unnormalize(action[3], -100, 100)
cf_posititon = self.get_position(1)
# Bounce drone off right wall
if cf_posititon[0] >= 4.5 and action[0] > 0:
action[0] = 0
cf_posititon = self.get_position(1)
# Bounce drone off left wall
elif cf_posititon[0] <= -4.5 and action[0] < 0:
action[0] = 0
cf_posititon = self.get_position(1)
# Bounce drone off back wall
if cf_posititon[1] >= 4.5 and action[1] > 0:
action[1] = 0
cf_posititon = self.get_position(1)
# Bounce drone off front wall
elif cf_posititon[1] <= -4.5 and action[1] < 0:
action[1] = 0
cf_posititon = self.get_position(1)
# Option 1: Hovering movements
action_msg = Hover()
action_msg.vx = action[0]
action_msg.vy = action[1]
action_msg.zDistance = action[2]
# action_msg.yawrate = action[3]
action_msg.yawrate = 0
# Option 2: Velocity movements
# action_msg = Twist()
# action_msg.linear.x = action[0]
# action_msg.linear.y = action[1]
# action_msg.linear.z = action[2]
# action_msg.angular.z = 0
# Option 3: Positon movements
# action_msg = Position()
# action_msg.x = action[0]
# action_msg.y = action[1]
# action_msg.z = action[2]
# action_msg.yaw = 0
return action_msg
def process_action(self, action):
""" Converts an array of actions into the necessary ROS msg type.
Args:
action (ndarray): Array containing the desired velocties along the
x, y, and z axes.
Returns:
Hover: ROS msg type necessary to publish a velocity command.
"""
cf_posititon = self.get_position(self.cf_id)
# Bounce drone off right wall
if cf_posititon[0] >= 4.5 and action[0] > 0:
action[0] = 0
cf_posititon = self.get_position(self.cf_id)
# Bounce drone off left wall
elif cf_posititon[0] <= -4.5 and action[0] < 0:
action[0] = 0
cf_posititon = self.get_position(self.cf_id)
# Bounce drone off back wall
if cf_posititon[1] >= 4.5 and action[1] > 0:
action[1] = 0
cf_posititon = self.get_position(self.cf_id)
# Bounce drone off front wall
elif cf_posititon[1] <= -4.5 and action[1] < 0:
action[1] = 0
# cf_posititon = self.get_position()
# Option 1: Hovering movements
action_msg = Hover()
action_msg.vx = action[0]
action_msg.vy = action[1]
action_msg.zDistance = action[2]
action_msg.yawrate = 0
return action_msg
def reset(self):
self.gazebo.unpauseSim()
# Known issue with cmd_vel(), so we must send a few zero velocities initially
# in order to use cmd_vel in step()
# action_msg = Twist()
# action_msg.linear.x = 0 # pitch
# action_msg.linear.y = 0 # roll
# action_msg.linear.z = 0 # thrust
# action_msg.angular.z = 0 # yaw
# for _ in range(3):
# self.vel_pub.publish(action_msg)
# time.sleep(0.3)
self.steps = 0
print('Start Reset')
reset_positions = self.random_position(-4, 5, 1, 10, 1)
action_msg = Hover()
action_msg.vx = 0
action_msg.vy = 0
action_msg.zDistance = reset_positions[0][2]
action_msg.yawrate = 0
while abs(self.get_position()[2] - reset_positions[0][2]) > 0.5:
self.hover_pub.publish(action_msg)
time.sleep(0.3)
# check if drone flipped during the reset
roll = self.get_roll_pitch_yaw()[0]
if abs(roll) >= 60:
self.kill_sim()
time.sleep(20)
self.gazebo_process, self.cf_process = self.launch_sim()
action_msg = Position()
action_msg.x = reset_positions[0][0]
action_msg.y = reset_positions[0][1]
action_msg.z = reset_positions[0][2]
action_msg.yaw = 0
while self.distance_between_points(self.get_position(),
reset_positions[0]) > 1:
self.position_pub.publish(action_msg)
time.sleep(0.3)
# check if drone flipped during the reset
roll = self.get_roll_pitch_yaw()[0]
if abs(roll) >= 60:
self.kill_sim()
time.sleep(20)
self.gazebo_process, self.cf_process = self.launch_sim()
print('End Reset')
# Connect to Crazyflie and enable high-level control
# self.cf = crazyflie.Crazyflie("cf1", "/cf1")
# self.cf.setParam("commander/enHighLevel", 1)
# reset_positions = self.random_position(-4, 5, 1, 10, 1)
# print('Start Reset')
# if self.first_reset:
# self.cf.takeoff(targetHeight = reset_positions[0][2], duration = 4)
# time.sleep(4)
# # check if need to get individual values from np array
# self.cf.goTo(goal=[reset_positions[0][0], reset_positions[0][1], reset_positions[0][2]], yaw=0.0, duration=4)
# time.sleep(4)
# print('End Reset')
observation = self.get_observation()
self.gazebo.pauseSim()
# self.first_reset = False
return observation
def process_action(self, action):
""" Converts an array of actions into the necessary ROS msg type.
Args:
action (ndarray): Array containing the desired velocties along the
x, y, and z axes.
Returns:
Hover: ROS msg type necessary to publish a velocity command.
"""
action[0] = self.unnormalize(action[0], -0.4, 0.4)
action[1] = self.unnormalize(action[1], -0.4, 0.4)
action[2] = self.unnormalize(action[2], 0.25, 9.75)
# action[3] = self.unnormalize(action[3], -200, 200)
# action[0] = self.unnormalize(action[0], -30, 30)
# action[1] = self.unnormalize(action[1], -30, 30)
# action[2] = self.unnormalize(action[2], 10000, 60000)
# action[3] = self.unnormalize(action[3], -200, 200)
cf_posititon = self.get_position()
# Bounce drone off right wall
if cf_posititon[0] >= 4.5 and action[0] > 0:
action[0] = 0
cf_posititon = self.get_position()
# Bounce drone off left wall
elif cf_posititon[0] <= -4.5 and action[0] < 0:
action[0] = 0
cf_posititon = self.get_position()
# Bounce drone off back wall
if cf_posititon[1] >= 4.5 and action[1] > 0:
action[1] = 0
cf_posititon = self.get_position()
# Bounce drone off front wall
elif cf_posititon[1] <= -4.5 and action[1] < 0:
action[1] = 0
cf_posititon = self.get_position()
# # Bounce drone off ceiling
# if cf_posititon[2] >= 9 and action[2] > 35000:
# action[2] = 30000
# cf_posititon = self.get_position()
# # Bounce drone off floor
# elif cf_posititon[2] <= 1 and action[2] < 35000:
# print('up!')
# action[2] = 60000
# Option 1: Hovering movements
action_msg = Hover()
action_msg.vx = action[0]
action_msg.vy = action[1]
action_msg.zDistance = action[2]
# action_msg.yawrate = action[3]
action_msg.yawrate = 0
# Option 2: Velocity movements
# action_msg = Twist()
# action_msg.linear.x = action[0] # pitch
# action_msg.linear.y = action[1] # roll
# action_msg.linear.z = action[2] # thrust
# action_msg.angular.z = 0 # yaw
# Option 3: Positon movements
# action_msg = Position()
# action_msg.x = action[0]
# action_msg.y = action[1]
# action_msg.z = action[2]
# action_msg.yaw = 0
return action_msg
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)
#for velocity/hover mode
msgHov = Hover()
msgHov.header.seq = 0
msgHov.header.stamp = rospy.Time.now()
msgHov.header.frame_id = worldFrame
msgHov.vx = 0.0
msgHov.vy = 0.0
msgHov.zDistance = 0.0
msgHov.yawrate = 0.0
pubHov = rospy.Publisher("cmd_hover", Hover, queue_size=1)
#to stop, don't really need this?
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)
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Vector3
from crazyflie_driver.msg import Hover
rospy.init_node('test')
# pub = rospy.Publisher('crazyflie1/cmd_vel', Twist, queue_size=0)
rate = rospy.Rate(2)
pub = rospy.Publisher('cf1/cmd_hover/', Hover, queue_size=0)
# t = Twist() # instatiate a command
# t.linear = Vector3(0, 0, 40000)
# t.angular = Vector3(0, 0, 10)
msg = Hover()
msg.vx = 0.0
msg.vy = 0.0
msg.yawrate = 0
msg.zDistance = 0.5
msg.header.seq = 1
msg.header.stamp = rospy.Time.now()
# pub.publish(msg)
# rate.sleep()
while not rospy.is_shutdown():
pub.publish(msg)
rate.sleep()