diff --git a/docs/howto/distributed_operation.rstinclude b/docs/howto/distributed_operation.rstinclude
new file mode 100644
index 000000000..5c9da0f60
--- /dev/null
+++ b/docs/howto/distributed_operation.rstinclude
@@ -0,0 +1,51 @@
+.. _tutorial_distributed_operation:
+
+Distributed Operation
+---------------------
+
+Consider the case where you would like to test a distributed algorithm without
+modifying the on-board firmware. This might be useful for development purposes
+or if your algorithm relies on computation- or memory-heavy operations.
+
+Here, we consider this use-case, where one process (or to be more precise: one ROS node)
+is executed for each robot on the host computer. This process can take the absolute or relative
+state of other robots into account and compute a setpoint (e.g., position) for its assigned
+Crazyflie.
+
+Step 1: Implement the Distributed Algorithm
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+If you use Python, you may base your work of an existing example script in `ros_ws/src/crazyswarm/scripts/example_distributed.py`.
+Note that you can't use all the features of `pycrazyswarm` anymore. In particular, you have to initialize your ROS node manually
+and add some logic to determine which Crazyflie should be serviced.
+
+If you use C++, you can directly use the ROS topics and services to interact with individual drones.
+
+To estimate the current absolute or relative state of other robots, you can rely on the ROS transform feature.
+
+Step 2: Adjust the Launch File
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+An example launch file is provided in `ros_ws/src/crazyswarm/launch/example_distributed.launch`.
+This includes `hover_swarm.launch` directly and therefore inherits any global Crazyswarm settings from there.
+Note that you have to specify one node for each Crazyflie. Each node should have some logic to simply exit if the
+corresponding Crazyflie is currently deactivated (this logic is part of the provided `example_distributed.py`).
+
+Step 3: Run in Simulation
+^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Since there is no single user script, one can not run the script with `--sim` anymore. However,
+there is a second simulation backend, which can be conveniently used with any launch file.::
+
+ source ros_ws/devel/setup.bash
+ roslaunch crazyswarm example_distributed.launch sim:=1
+
+The simulator uses the same software-in-the-loop code without any physics engine. The visualization is now done directly
+in `rviz`, rather than `matplotlib` or `vispy`.
+
+Step 4: Run on Robots
+^^^^^^^^^^^^^^^^^^^^^
+
+The same launch file can be used to run on the real robots by not using the `sim` arguments.::
+
+ roslaunch crazyswarm example_distributed.launch
\ No newline at end of file
diff --git a/docs/howto/howto.rst b/docs/howto/howto.rst
index 05a25d7b8..0c00f1af0 100644
--- a/docs/howto/howto.rst
+++ b/docs/howto/howto.rst
@@ -1,6 +1,8 @@
How-To Guides
=============
+.. include:: distributed_operation.rstinclude
+
.. include:: git_integration.rstinclude
.. include:: streaming_setpoint.rstinclude
diff --git a/docs/images/software_architecture.png b/docs/images/software_architecture.png
index 378ab55b2..56e225863 100644
Binary files a/docs/images/software_architecture.png and b/docs/images/software_architecture.png differ
diff --git a/docs/images/software_architecture.svg b/docs/images/software_architecture.svg
index a47881cc9..476d7e6c9 100644
--- a/docs/images/software_architecture.svg
+++ b/docs/images/software_architecture.svg
@@ -9,6 +9,9 @@
id="svg5"
inkscape:version="1.1.1 (1:1.1+202109281943+c3084ef5ed)"
sodipodi:docname="software_architecture.svg"
+ inkscape:export-filename="/home/whoenig/projects/crazyflie/crazyswarm/docs/images/software_architecture.png"
+ inkscape:export-xdpi="150"
+ inkscape:export-ydpi="150"
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
xmlns:xlink="http://www.w3.org/1999/xlink"
@@ -24,9 +27,9 @@
inkscape:pagecheckerboard="0"
inkscape:document-units="mm"
showgrid="false"
- inkscape:zoom="1.2810466"
- inkscape:cx="380.54822"
- inkscape:cy="282.97175"
+ inkscape:zoom="0.90583674"
+ inkscape:cx="508.36975"
+ inkscape:cy="245.62925"
inkscape:window-width="1848"
inkscape:window-height="1136"
inkscape:window-x="72"
@@ -299,7 +302,7 @@
fx="55.106754"
fy="25.465508"
r="25.948801"
- gradientTransform="matrix(2.0562639,-0.05009649,0.00748322,0.30715709,-0.07785726,64.456283)"
+ gradientTransform="matrix(2.0562639,-0.05009649,0.00748322,0.30715709,45.715028,63.839269)"
gradientUnits="userSpaceOnUse" />
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -672,14 +810,14 @@
id="g14-9">
@@ -766,14 +904,14 @@
id="g14-9-6">
@@ -822,14 +960,14 @@
id="g14-9-6-3">
@@ -878,62 +1016,17 @@
id="g14-9-4">
-
-
-
-
-
-
-
-
-
-
-
-
+
+
-
- pycrazyswarmw/ Simulator
+ id="g14862"
+ transform="translate(0,0.77000427)">
+ id="g57"
+ transform="matrix(0.03449891,0,0,0.03449891,167.93245,64.3926)">
+
+
+
+
+
+
+
+
+
+
+
+
+ pycrazyswarm
+
+ id="surface839-7"
+ transform="translate(-0.0585938,-0.894531)">
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ d="m 184.6,61.93 c 0,-14.56 -4.152,-22.04 -12.46,-22.45 -3.304,-0.156 -6.531,0.3704 -9.669,1.589 -2.505,0.8967 -4.191,1.784 -5.078,2.68 v 34.76 c 5.312,3.334 10.03,4.883 14.14,4.64 8.704,-0.5751 13.06,-7.642 13.06,-21.22 z m 10.24,0.6043 c 0,7.398 -1.735,13.54 -5.224,18.42 -3.889,5.527 -9.279,8.373 -16.17,8.529 -5.195,0.1657 -10.55,-1.462 -16.05,-4.874 v 31.59 l -8.909,-3.178 v -70.12 c 1.462,-1.793 3.343,-3.334 5.624,-4.64 5.302,-3.09 11.75,-4.679 19.33,-4.757 l 0.1267,0.1267 c 6.93,-0.08773 12.27,2.758 16.02,8.529 3.499,5.293 5.254,12.08 5.254,20.37 z"
+ style="fill:#646464"
+ id="path23-0" />
+ d="m 249.3,83.27 c 0,9.923 -0.9942,16.79 -2.983,20.62 -1.998,3.821 -5.8,6.872 -11.41,9.143 -4.552,1.793 -9.474,2.768 -14.76,2.934 l -1.472,-5.614 c 5.371,-0.731 9.153,-1.462 11.35,-2.193 4.318,-1.462 7.281,-3.704 8.909,-6.706 1.306,-2.447 1.949,-7.115 1.949,-14.03 v -2.32 c -6.092,2.768 -12.48,4.143 -19.15,4.143 -4.386,0 -8.256,-1.374 -11.59,-4.143 -3.743,-3.012 -5.614,-6.833 -5.614,-11.46 v -37.08 l 8.909,-3.051 v 37.32 c 0,3.987 1.287,7.057 3.86,9.211 2.573,2.154 5.907,3.187 9.991,3.109 4.084,-0.08773 8.46,-1.667 13.11,-4.757 v -43.54 h 8.909 v 48.41 z"
+ style="fill:#646464"
+ id="path25-2" />
+ d="m 284.1,89 c -1.062,0.08772 -2.037,0.1267 -2.934,0.1267 -5.039,0 -8.967,-1.199 -11.77,-3.606 -2.797,-2.408 -4.201,-5.731 -4.201,-9.971 v -35.09 h -6.102 v -5.605 h 6.102 v -14.88 l 8.899,-3.168 v 18.05 h 10.01 v 5.605 h -10.01 v 34.85 c 0,3.343 0.8967,5.712 2.69,7.096 1.54,1.14 3.987,1.793 7.32,1.959 v 4.64 z"
+ style="fill:#646464"
+ id="path27-3" />
+ d="m 338,88.27 h -8.909 V 53.88 c 0,-3.499 -0.8188,-6.511 -2.447,-9.026 -1.881,-2.846 -4.493,-4.269 -7.846,-4.269 -4.084,0 -9.192,2.154 -15.32,6.462 v 41.22 h -8.909 V 6.067 L 303.478,3.26 V 40.7 c 5.692,-4.143 11.91,-6.219 18.67,-6.219 4.718,0 8.538,1.589 11.46,4.757 2.934,3.168 4.396,7.115 4.396,11.83 v 37.19 z"
+ style="fill:#646464"
+ id="path29-7" />
+ d="m 385.4,60.53 c 0,-5.595 -1.062,-10.21 -3.178,-13.87 -2.515,-4.454 -6.423,-6.803 -11.71,-7.047 -9.767,0.5653 -14.64,7.564 -14.64,20.98 0,6.15 1.014,11.29 3.061,15.41 2.612,5.254 6.531,7.846 11.76,7.759 9.806,-0.07798 14.71,-7.817 14.71,-23.23 z m 9.757,0.05848 c 0,7.963 -2.037,14.59 -6.102,19.88 -4.474,5.926 -10.65,8.899 -18.54,8.899 -7.817,0 -13.91,-2.973 -18.31,-8.899 -3.987,-5.293 -5.975,-11.92 -5.975,-19.88 0,-7.486 2.154,-13.78 6.462,-18.91 4.552,-5.439 10.54,-8.168 17.93,-8.168 7.398,0 13.42,2.729 18.06,8.168 4.308,5.127 6.462,11.42 6.462,18.91 z"
+ style="fill:#646464"
+ id="path31-5" />
+ d="m 446.2,88.27 h -8.909 V 51.93 c 0,-3.987 -1.199,-7.096 -3.597,-9.338 -2.398,-2.232 -5.595,-3.314 -9.581,-3.226 -4.23,0.07798 -8.256,1.462 -12.08,4.143 v 44.76 h -8.909 v -45.86 c 5.127,-3.733 9.845,-6.17 14.15,-7.31 4.065,-1.062 7.651,-1.589 10.74,-1.589 2.115,0 4.104,0.2047 5.975,0.6141 3.499,0.809 6.345,2.31 8.538,4.513 2.447,2.437 3.665,5.361 3.665,8.782 v 40.85 z"
+ style="fill:#646464"
+ id="path33-9" />
-
-
-
-
-
-
+ d="M 60.51,6.398 C 55.926,6.4193 51.549,6.8102 47.7,7.492 36.35,9.497 34.29,13.692 34.29,21.432 v 10.22 H 61.1 v 3.406 H 34.29 24.23 c -7.792,0 -14.62,4.684 -16.75,13.59 -2.462,10.21 -2.571,16.59 0,27.25 1.906,7.938 6.458,13.59 14.25,13.59 h 9.219 v -12.25 c 0,-8.85 7.657,-16.66 16.75,-16.66 h 26.78 c 7.455,0 13.41,-6.138 13.41,-13.62 v -25.53 c 0,-7.266 -6.13,-12.72 -13.41,-13.94 C 69.873,6.7213 65.094,6.373 60.509,6.394 Z m -14.5,8.219 c 2.77,0 5.031,2.299 5.031,5.125 -2e-6,2.816 -2.262,5.094 -5.031,5.094 -2.779,-10e-7 -5.031,-2.277 -5.031,-5.094 -10e-7,-2.826 2.252,-5.125 5.031,-5.125 z"
+ style="fill:url(#linearGradient1445)"
+ id="path35-2" />
+
+
+
+
+ Simulator
`. These tools include methods to list logging variables, parameters, and to reboot individual crazyflies.
- **crazyswarm_server**
This application is the core of the Crazyswarm. It provides the ROS interface, communicates with the robots and the motion capture system.
+- **Simulator**
+ The simulator uses parts of the official firmware for software-in-the-loop simulation. For performance reasons, the simulation does not include the dynamics and rather visualizes the setpoints. The simulator provides the same interface as the crazyswarm_server. Thus, users can either use the simulator or the physical hardware as backend.
- **pycrazyswarm**
- This is a simplified Python library to use the Crazyswarm. It has two backends: the physical backend (communicating with the crazyswarm_server) and the simulation backend. The simulator uses parts of the official firmware for software-in-the-loop simulation. For performance reasons, the simulation does not include the dynamics and rather visualizes the setpoints.
+ This is a simplified Python library to use the Crazyswarm. It has two backends: the physical backend (communicating with the crazyswarm_server) and the simulation backend.
- **Helper libraries**
We provide a unified interface for different motion capture systems (`libMotionCapture`), a way to track rigid bodies frame-by-frame even with unique marker configurations (`libObjectTracker`), and a library for the low-level communication with the Crazyflie robots (`crazyflie_cpp`).
diff --git a/ros_ws/src/crazyswarm/launch/example_distributed.launch b/ros_ws/src/crazyswarm/launch/example_distributed.launch
new file mode 100644
index 000000000..34168740d
--- /dev/null
+++ b/ros_ws/src/crazyswarm/launch/example_distributed.launch
@@ -0,0 +1,13 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/ros_ws/src/crazyswarm/launch/hover_swarm.launch b/ros_ws/src/crazyswarm/launch/hover_swarm.launch
index be77e2543..66a532ff9 100644
--- a/ros_ws/src/crazyswarm/launch/hover_swarm.launch
+++ b/ros_ws/src/crazyswarm/launch/hover_swarm.launch
@@ -1,11 +1,13 @@
+
-
+
+
# Logging configuration (Use enable_logging to actually enable logging)
genericLogTopics: ["log1"]
@@ -46,6 +48,10 @@
+
+
+
+
diff --git a/ros_ws/src/crazyswarm/scripts/example_distributed.py b/ros_ws/src/crazyswarm/scripts/example_distributed.py
new file mode 100755
index 000000000..433df539f
--- /dev/null
+++ b/ros_ws/src/crazyswarm/scripts/example_distributed.py
@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+
+import rospy
+from tf import TransformListener
+import numpy as np
+
+from pycrazyswarm.crazyflie import Crazyflie
+
+def run(tf, cf):
+ # Advanced users: Use the following to get state information of neighbors
+ # position, quaternion = tf.lookupTransform("/world", "/cf" + str(cfid), rospy.Time(0))
+ pos = cf.initialPosition.copy()
+ cf.takeoff(targetHeight=0.5, duration=2.0)
+ for _ in range(10):
+ pos[2] = np.random.uniform(0.5, 1.0)
+ duration = np.random.uniform(0.5, 2.0)
+ rospy.loginfo("CF {} going to {} in {}s".format(cf.id, pos, duration))
+ cf.goTo(pos, 0, duration)
+ rospy.sleep(duration)
+
+ cf.land(targetHeight=0.02, duration=2.0)
+
+if __name__ == "__main__":
+
+ rospy.init_node("CrazyflieDistributed", anonymous=True)
+ cfid = rospy.get_param("~cfid")
+ cf = None
+ for crazyflie in rospy.get_param("crazyflies"):
+ if cfid == int(crazyflie["id"]):
+ initialPosition = crazyflie["initialPosition"]
+ tf = TransformListener()
+ cf = Crazyflie(cfid, initialPosition, tf)
+ break
+
+ if cf is None:
+ rospy.logwarn("No CF with required ID {} found!".format(cfid))
+ else:
+ run(tf, cf)
diff --git a/ros_ws/src/crazyswarm/scripts/sim_ros.py b/ros_ws/src/crazyswarm/scripts/sim_ros.py
new file mode 100755
index 000000000..231d03eca
--- /dev/null
+++ b/ros_ws/src/crazyswarm/scripts/sim_ros.py
@@ -0,0 +1,234 @@
+#!/usr/bin/env python3
+
+import rospy
+import tf2_ros
+from tf.transformations import euler_from_quaternion
+from std_msgs.msg import Empty as EmptyMsg
+from std_srvs.srv import Empty as EmptySrv
+import geometry_msgs.msg
+from visualization_msgs.msg import Marker
+
+import numpy as np
+from crazyswarm.srv import *
+from crazyswarm.msg import TrajectoryPolynomialPiece, FullState, Position, VelocityWorld
+from pycrazyswarm.crazyflieSim import Crazyflie, CrazyflieServer
+import uav_trajectory
+
+
+class TimeHelperROS:
+ def __init__(self):
+ self.start = rospy.Time.now()
+
+ def time(self):
+ return (rospy.Time.now() - self.start).to_sec()
+
+
+class CrazyflieROSSim:
+ def __init__(self, id, initialPosition, timeHelper):
+ self.cfsim = Crazyflie(id, initialPosition, timeHelper)
+
+ prefix = "/cf" + str(id)
+ rospy.Service(prefix + '/set_group_mask', SetGroupMask, self.handle_set_group_mask)
+ rospy.Service(prefix + '/takeoff', Takeoff, self.handle_takeoff)
+ rospy.Service(prefix + '/land', Land, self.handle_land)
+ rospy.Service(prefix + '/go_to', GoTo, self.handle_go_to)
+ rospy.Service(prefix + '/upload_trajectory', UploadTrajectory, self.handle_upload_trajectory)
+ rospy.Service(prefix + '/start_trajectory', StartTrajectory, self.handle_start_trajectory)
+ rospy.Service(prefix + '/notify_setpoints_stop', NotifySetpointsStop, self.handle_notify_setpoints_stop)
+ rospy.Service(prefix + '/update_params', UpdateParams, self.handle_update_params)
+
+ rospy.Subscriber(prefix + "/cmd_full_state", FullState, self.handle_cmd_full_state)
+ rospy.Subscriber(prefix + "/cmd_position", Position, self.handle_cmd_position)
+ rospy.Subscriber(prefix + "/cmd_stop", EmptyMsg, self.handle_cmd_stop)
+
+ # LED support
+ self.ledsPublisher = rospy.Publisher("/visualization_marker", Marker, queue_size=1)
+
+ # hacky stop support
+ self.stopped = False
+
+ def handle_set_group_mask(self, req):
+ self.cfsim.setGroupMask(req.groupMask)
+ return SetGroupMaskResponse()
+
+ def handle_takeoff(self, req):
+ self.cfsim.takeoff(req.height, req.duration.to_sec(), req.groupMask)
+ return TakeoffResponse()
+
+ def handle_land(self, req):
+ self.cfsim.land(req.height, req.duration.to_sec(), req.groupMask)
+ return LandResponse()
+
+ def handle_go_to(self, req):
+ goal = [req.goal.x, req.goal.y, req.goal.z]
+ self.cfsim.goTo(goal, req.yaw, req.duration.to_sec(), req.relative, req.groupMask)
+ return GoToResponse()
+
+ def handle_upload_trajectory(self, req):
+ trajectory = uav_trajectory.Trajectory()
+ trajectory.duration = 0
+ trajectory.polynomials = []
+ for piece in req.pieces:
+ poly = uav_trajectory.Polynomial4D(
+ piece.duration.to_sec(), piece.poly_x, piece.poly_y, piece.poly_z, piece.poly_yaw)
+ trajectory.polynomials.append(poly)
+ trajectory.duration += poly.duration
+ self.cfsim.uploadTrajectory(req.trajectoryId, req.pieceOffset, trajectory)
+ return UploadTrajectoryResponse()
+
+ def handle_start_trajectory(self, req):
+ self.cfsim.startTrajectory(req.trajectoryId, req.timescale, req.reversed, req.relative, req.groupMask)
+ return StartTrajectoryResponse()
+
+ def handle_notify_setpoints_stop(self, req):
+ self.cfsim.notifySetpointsStop(req.remainValidMillisecs)
+ return NotifySetpointsStopResponse()
+
+ def handle_update_params(self, req):
+ for param in req.params:
+ if "ring/solid" in param:
+ self.updateLED()
+ if param == "ring/effect":
+ v = rospy.get_param("/cf" + str(self.id) + "/ring/effect")
+ if v == 0:
+ self.removeLED()
+ else:
+ rospy.logwarn("Updating param {} not implemented in simulation!".format(param))
+
+ return UpdateParamsResponse()
+
+ def handle_cmd_full_state(self, msg):
+ pos = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
+ vel = [msg.twist.linear.x, msg.twist.linear.y, msg.twist.linear.z]
+ acc = [msg.acc.x, msg.acc.y, msg.acc.z]
+ omega = [msg.twist.angular.x, msg.twist.angular.y, msg.twist.angular.z]
+ xyzw = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]
+ _, _, yaw = euler_from_quaternion(xyzw)
+ self.cfsim.cmdFullState(pos, vel, acc, yaw, omega)
+
+ def handle_cmd_position(self, msg):
+ pos = [msg.x, msg.y, msg.z]
+ self.cfsim.cmdPosition(pos, msg.yaw)
+
+ def handle_cmd_stop(self, msg):
+ self.cfsim.cmdStop()
+ self.stopped = True
+ self.removeLED()
+
+ def removeLED(self):
+ marker = Marker()
+ marker.header.frame_id = "cf" + str(self.id)
+ marker.ns = "LED"
+ marker.id = self.id
+ marker.action = marker.DELETE
+ self.ledsPublisher.publish(marker)
+
+ def updateLED(self):
+
+ if rospy.has_param("/cf" + str(self.id) + "/ring/solidRed") and \
+ rospy.has_param("/cf" + str(self.id) + "/ring/solidGreen") and \
+ rospy.has_param("/cf" + str(self.id) + "/ring/solidBlue"):
+
+ marker = Marker()
+ marker.header.frame_id = "cf" + str(self.id)
+ marker.ns = "LED"
+ marker.id = self.id
+ marker.type = marker.SPHERE
+ marker.action = marker.ADD
+ marker.scale.x = 0.3
+ marker.scale.y = 0.3
+ marker.scale.z = 0.3
+ marker.color.a = 0.2
+ marker.color.r = rospy.get_param("/cf" + str(self.id) + "/ring/solidRed")
+ marker.color.g = rospy.get_param("/cf" + str(self.id) + "/ring/solidGreen")
+ marker.color.b = rospy.get_param("/cf" + str(self.id) + "/ring/solidBlue")
+ marker.pose.orientation.w = 1.0
+ marker.frame_locked = True
+ self.ledsPublisher.publish(marker)
+
+
+class CrazyflieServerROSSim:
+ def __init__(self, timehelper):
+ """Initialize the server.
+ """
+ # Populate crazyflies
+ self.crazyflies = []
+ for crazyflie in rospy.get_param("crazyflies"):
+ id = int(crazyflie["id"])
+ initialPosition = crazyflie["initialPosition"]
+ cf = CrazyflieROSSim(id, initialPosition, timehelper)
+ self.crazyflies.append(cf)
+
+ rospy.Service('/emergency', EmptySrv, self.handle_emergency)
+ rospy.Service('/takeoff', Takeoff, self.handle_takeoff)
+ rospy.Service('/land', Land, self.handle_land)
+ rospy.Service('/go_to', GoTo, self.handle_go_to)
+ rospy.Service('/start_trajectory', StartTrajectory, self.handle_start_trajectory)
+ rospy.Service('/update_params', UpdateParams, self.handle_update_params)
+
+ def handle_emergency(self, req):
+ rospy.logwarn("Emergency not implemented in simulation!")
+ return EmptyResponse()
+
+ def handle_takeoff(self, req):
+ for cf in self.crazyflies:
+ cf.handle_takeoff(req)
+ return TakeoffResponse()
+
+ def handle_land(self, req):
+ for cf in self.crazyflies:
+ cf.handle_land(req)
+ return LandResponse()
+
+ def handle_go_to(self, req):
+ for cf in self.crazyflies:
+ cf.handle_go_to(req)
+ return GoToResponse()
+
+ def handle_start_trajectory(self, req):
+ for cf in self.crazyflies:
+ cf.handle_start_trajectory(req)
+ return StartTrajectoryResponse()
+
+ def handle_update_params(self, req):
+ for cf in self.crazyflies:
+ cf.handle_update_params(req)
+ return UpdateParamsResponse()
+
+
+def main():
+ rospy.init_node("CrazyflieROSSim", anonymous=False)
+ dt = rospy.get_param('dt', 0.1)
+
+ timeHelper = TimeHelperROS()
+ srv = CrazyflieServerROSSim(timeHelper)
+
+ rate = rospy.Rate(1/dt) # hz
+
+ br = tf2_ros.TransformBroadcaster()
+ transform = geometry_msgs.msg.TransformStamped()
+ transform.header.frame_id = "world"
+ transform.transform.rotation.x = 0
+ transform.transform.rotation.y = 0
+ transform.transform.rotation.z = 0
+ transform.transform.rotation.w = 1
+
+ while not rospy.is_shutdown():
+ transform.header.stamp = rospy.Time.now()
+ for cf in srv.crazyflies:
+ cf.cfsim.integrate(dt, 0, np.inf)
+ if not cf.stopped:
+ cfid = cf.cfsim.id
+ pos = cf.cfsim.position()
+ if np.isfinite(pos).all():
+ transform.child_frame_id = "/cf" + str(cfid)
+ transform.transform.translation.x = pos[0]
+ transform.transform.translation.y = pos[1]
+ transform.transform.translation.z = pos[2]
+ br.sendTransform(transform)
+ for cf in srv.crazyflies:
+ cf.cfsim.flip()
+ rate.sleep()
+
+if __name__ == "__main__":
+ main()