def __init__(self, prefix):
self.prefix = prefix
worldFrame = rospy.get_param("~worldFrame", "/world")
self.rate = rospy.Rate(10)
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.pub = rospy.Publisher(prefix + "/cmd_hover", Hover, queue_size=1)
self.msg = Hover()
self.msg.header.seq = 0
self.msg.header.stamp = rospy.Time.now()
self.msg.header.frame_id = worldFrame
self.msg.yawrate = 0
self.stop_pub = rospy.Publisher(prefix + "/cmd_stop",
Stop,
queue_size=1)
self.stop_msg = Stop()
self.stop_msg.header.seq = 0
self.stop_msg.header.stamp = rospy.Time.now()
self.stop_msg.header.frame_id = worldFrame
def __init__(self, prefix):
self.prefix = prefix
worldFrame = rospy.get_param("~worldFrame", "/world")
self.rate = rospy.Rate(10)
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.pub = rospy.Publisher(prefix + "/cmd_hover", Hover, queue_size=1)
self.msg = Hover()
self.msg.header.seq = 0
self.msg.header.stamp = rospy.Time.now()
self.msg.header.frame_id = worldFrame
self.msg.yawrate = 0
self.stop_pub = rospy.Publisher(prefix + "/cmd_stop",
Empty,
queue_size=1)
self.stop_msg = Empty()
# change thrush
self.twist_up = Twist()
self.twist_up.linear.z = 38500
self.twist_stay = Twist()
self.twist_stay.linear.z = 35500
self.thrust_pub = rospy.Publisher(prefix + '/cmd_vel',
Twist,
queue_size=1)
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 __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):
rospy.init_node('test')
# worldFrame = rospy.get_param("~worldFrame", "/world")
self.msg = Hover()
self.twist_msg = Twist()
self.duration_msg = Duration()
self.hz = 100 # if not set to 100, will not broadcast
self.rate = rospy.Rate(self.hz)
# rospy.wait_for_service('/crazyflie/takeoff')
# self.takeoff_command = rospy.ServiceProxy('/crazyflie/takeoff', Takeoff)
self.pub_twist = rospy.Publisher('crazyflie/cmd_vel', Twist, queue_size=0)
rospy.wait_for_service('/vicon/grab_vicon_pose')
self.pose_getter = rospy.ServiceProxy('/vicon/grab_vicon_pose', viconGrabPose)
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
except Exception as e:
cf_handler.stop()
rospy.loginfo('*******keyboard input exception')
rospy.loginfo(e)
if __name__ == '__main__':
rospy.init_node('keyboard_controller')
prefix = rospy.get_param("~tf_prefix")
rospy.Subscriber('/' + prefix + '/log_ranges', GenericLogData, get_ranges)
pub_hover = rospy.Publisher(prefix + "/cmd_hover", Hover,
queue_size=1) #hover message publisher
msg = Hover()
msg.header.seq = 0
cf = crazyflie.Crazyflie("/" + prefix, "world")
rospy.wait_for_service("/" + prefix + '/update_params')
rospy.loginfo("found update_params service")
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("ctrlMel/kp_z", 0.8) # reduce z wobble - default 1.25
# cf.setParam("ctrlMel/ki_z", 0.06) #reduce z wobble - default 0.05
# cf.setParam("ctrlMel/kd_z", 0.2) #reduce z wobble - default 0.4
# cf.setParam("ctrlMel/i_range_z", 0.2) #reduce z wobble
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 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
#!/usr/bin/env python
import rospy
import sys
import math
from crazyflie_driver.msg import FullState
from crazyflie_driver.msg import Hover
if __name__ == '__main__':
pub = rospy.Publisher('/cf1/cmd_hover', Hover, queue_size=5)
rospy.init_node('rviz')
rate = rospy.Rate(10)
msg = Hover()
i = 0
while i < 60:
print i
msg.zDistance = 40
pub.publish(msg)
rate.sleep()
i = i + 2
msg.header.seq += 1
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rate.sleep()
zDistance -= 0.2
stop_pub.publish(stop_msg)
if __name__ == '__main__':
rospy.init_node('hover', anonymous=True)
worldFrame = rospy.get_param("~worldFrame", "/world")
rate = rospy.Rate(10) # 10 hz
name = "cmd_hover"
msg = Hover()
msg.header.seq = 0
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = worldFrame
msg.yawrate = 0
pub = rospy.Publisher(name, Hover, queue_size=1)
stop_pub = rospy.Publisher("cmd_stop", Stop, queue_size=1)
stop_msg = Stop()
stop_msg.header.seq = 0
stop_msg.header.stamp = rospy.Time.now()
stop_msg.header.frame_id = worldFrame
rospy.wait_for_service('update_params')
rospy.loginfo("found update_params service")
rate = rospy.Rate(10) # hz
rospy.Subscriber("log1", GenericLogData, callback_pos)
#x = rospy.get_param("kalman.stateX")
#print(" GOT X. X IS: "+ str(x) + "\n*****\n*****\n")
#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)
#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)
#!/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()