Add simple ROS-based simulator (using RVIZ for visualization)

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

docs/howto/howto.rst

@@ -1,6 +1,8 @@
 How-To Guides
 =============
 
+.. include:: distributed_operation.rstinclude
+
 .. include:: git_integration.rstinclude
 
 .. include:: streaming_setpoint.rstinclude

docs/overview.rst

@@ -7,15 +7,17 @@ Crazyswarm has the following software architecture.
 
     .. figure:: images/software_architecture.png
         :align: center
-        :scale: 70%
+        :scale: 50%
 
 
 - **crazyflie_tools**
   These are command line tools that can be used using `rosrun crazyflie_tools <name> <arguments>`. 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`).
 

ros_ws/src/crazyswarm/launch/example_distributed.launch

@@ -0,0 +1,13 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find crazyswarm)/launch/hover_swarm.launch" pass_all_args="true" />
+
+  <node name="cf1" pkg="crazyswarm" type="example_distributed.py">
+    <param name="cfid" value="1" />
+  </node>
+
+  <node name="cf2" pkg="crazyswarm" type="example_distributed.py">
+    <param name="cfid" value="2" />
+  </node>
+
+</launch>

ros_ws/src/crazyswarm/launch/hover_swarm.launch

@@ -1,11 +1,13 @@
 <?xml version="1.0"?>
 <launch>
+  <arg name="sim" default="false" />
   <arg name="joy_dev" default="/dev/input/js0" />
 
   <rosparam command="load" file="$(find crazyswarm)/launch/crazyflieTypes.yaml" />
   <rosparam command="load" file="$(find crazyswarm)/launch/crazyflies.yaml" />
 
-  <node pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
+  <!-- Run crazyswarm_server, if sim is set to False -->
+  <node unless="$(arg sim)" pkg="crazyswarm" type="crazyswarm_server" name="crazyswarm_server" output="screen" >
     <rosparam>
       # Logging configuration (Use enable_logging to actually enable logging)
       genericLogTopics: ["log1"]
@@ -46,6 +48,10 @@
     </rosparam>
   </node>
 
+  <!-- Run crazyswarm_server simulation, if sim is set to True -->
+  <node if="$(arg sim)" name="sim" pkg="crazyswarm" type="sim_ros.py" output="screen">
+  </node>
+
   <node name="joy" pkg="joy" type="joy_node" output="screen">
     <param name="dev" value="$(arg joy_dev)" />
   </node>

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)

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()