Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 8 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
51 changes: 51 additions & 0 deletions docs/howto/distributed_operation.rstinclude
Original file line number Diff line number Diff line change
@@ -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
2 changes: 2 additions & 0 deletions docs/howto/howto.rst
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
How-To Guides
=============

.. include:: distributed_operation.rstinclude

.. include:: git_integration.rstinclude

.. include:: streaming_setpoint.rstinclude
Binary file modified docs/images/software_architecture.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
527 changes: 371 additions & 156 deletions docs/images/software_architecture.svg
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
6 changes: 4 additions & 2 deletions docs/overview.rst
Original file line number Diff line number Diff line change
Expand Up @@ -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`).

Expand Down
13 changes: 13 additions & 0 deletions ros_ws/src/crazyswarm/launch/example_distributed.launch
Original file line number Diff line number Diff line change
@@ -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>
8 changes: 7 additions & 1 deletion ros_ws/src/crazyswarm/launch/hover_swarm.launch
Original file line number Diff line number Diff line change
@@ -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"]
Expand Down Expand Up @@ -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>
Expand Down
38 changes: 38 additions & 0 deletions ros_ws/src/crazyswarm/scripts/example_distributed.py
Original file line number Diff line number Diff line change
@@ -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)
234 changes: 234 additions & 0 deletions ros_ws/src/crazyswarm/scripts/sim_ros.py
Original file line number Diff line number Diff line change
@@ -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()