def __init__(self, id, initialPosition, tf):
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 + "/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)
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 cmdFullState(self, pose_data, twist_data, acc_data):
'''
pose_data: list/array with position and quaternion rotation (x, y, z, qx, qy, qz, qw)
twist_data: list/array with linear and angular velocities (vx, vy, vz, roll_rate, pitch_rate, yaw_rate)
acc_data: list/array with acceleration (ax, ay, az)
'''
state = FullState()
pose = geometry_msgs.msg.Pose()
twist = geometry_msgs.msg.Twist()
acc = geometry_msgs.msg.Vector3()
pose.position.x = pose_data[0]
pose.position.y = pose_data[1]
pose.position.z = pose_data[2]
pose.orientation.x = pose_data[3]
pose.orientation.y = pose_data[4]
pose.orientation.z = pose_data[5]
pose.orientation.w = pose_data[6]
twist.linear.x = twist_data[0]
twist.linear.y = twist_data[1]
twist.linear.z = twist_data[2]
twist.angular.x = twist_data[3]
twist.angular.y = twist_data[4]
twist.angular.z = twist_data[5]
acc.x = acc_data[0]
acc.y = acc_data[1]
acc.z = acc_data[2]
state.pose = pose
state.twist = twist
state.acc = acc
state.header.seq += 1
state.header.stamp = rospy.Time.now()
state.header.frame_id = self.worldFrame
self.pubCmdFullState.publish(state)
if __name__ == '__main__':
# parser = argparse.ArgumentParser()
# parser.add_argument("trajectory", type=str, help="CSV file containing trajectory")
# args = parser.parse_args()
rospy.init_node('full_state', anonymous=True)
traj = uav_trajectory.Trajectory()
traj.loadcsv(
'/home/alsarmi/Dropbox/KTH_Classes/Project_Course_Drone/dd2419_ws/src/milestone2/scripts/figure8withTakeoffAndLanding.csv'
)
rate = rospy.Rate(100)
msg = FullState()
msg.header.seq = 0
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
pub = rospy.Publisher("/cf1/cmd_full_state", FullState, queue_size=1)
start_time = rospy.Time.now()
while not rospy.is_shutdown():
msg.header.seq += 1
msg.header.stamp = rospy.Time.now()
t = (msg.header.stamp - start_time).to_sec()
print(t)
if t > traj.duration:
break
# t = traj.duration
