<> <> == Overview == This package contains the implementation of a gazebo simulator for the Ardrone 2.0 and has been written by Hongrong Huang and [[http://vision.in.tum.de/members/sturmju|Juergen Sturm]] of the [[http://vision.in.tum.de/|Computer Vision Group]] at the Technical University of Munich. This package is based on the ROS package [[http://www.ros.org/wiki/tu-darmstadt-ros-pkg/|tu-darmstadt-ros-pkg]] by Johannes Meyer and Stefan Kohlbrecher and the Ardrone simulator which is provided by Matthias Nieuwenhuisen. The simulator can simulate both the AR.Drone 1.0 and 2.0, the default parameters however are optimized for the AR.Drone 2.0 by now. You can fine more details on my report. The simulator is on the [[http://www.ros.org/wiki/simulator_gazebo?distro=fuerte/|simulator_gazebo]] platform. Before using this simulator, it is recommended to get know more about the simulator by reading [[http://www.ros.org/wiki/simulator_gazebo/Tutorials/|gazebo tutorials]]. Additionaly, you can get more information about flying robots and AR.Drone from the lecture [[http://cvpr.in.tum.de/teaching/ss2012/visnav2012/|visual navigation for flying robots]]. Real quadrocopter structure: The AR.Drone2.0 connects with a computer via WIFI, while the user manipulate a joystick which is via USB connecting with the same computer. ROS is running in this computer. {{attachment:real_structure.png}} Simulated quadrocopter manipulation: After the simulation is started. The user can use a joystick to manipulate the simulated quadrocopter. The botton functions of the joystick are the same as manipulating the real AR.Drone2.0. {{attachment:sim_structure.png}} The following video shows an experiment about how the simulator works compared to the real AR.Drone. <> == Nodes == * [[http://www.ros.org/wiki/simulator_gazebo?distro=fuerte/|gazebo]], ground_truth_to_tf, robot_state_publisher: The simulator program. * [[http://http://ros.org/wiki/rviz/|rviz]]: A 3d visualization environment for the simulated flying robots. * joy_node, ardrone_joystick: They are the joystick driver and the command information translator for the Ardrone. == Quickstart == === Installation === Your computer should have ROS and simulator gazebo package installed. The ROS version should be fuerte and the Gazebo version of the ROS plugin (which is, for fuerte, 1.0.2). Be sure that [[http://http://ros.org/wiki/rviz/|rviz]] is also installed. 1. Install the following additional required packages in your ROS workspace. * [[http://github.com/AutonomyLab/ardrone_autonomy#installation|ardrone_autonomy]] package: This is the official driver for the real Ardrone flying robots. {{{ $ cd ~/catkin_ws/src $ git clone https://github.com/AutonomyLab/ardrone_autonomy.git -b indigo-devel $ cd ~/catkin_ws $ rosdep install --from-paths src -i $ catkin_make }}} * joy_node and ardrone_joystick packages {{{ # cd into ros root dir roscd # clone repository svn checkout https://svncvpr.informatik.tu-muenchen.de/cvpr-ros-pkg/trunk/ardrone_helpers # add to ros path (if required) export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:`pwd`/ardrone_helpers # build package rosmake ardrone_joystick rosmake joy }}} 2. Install tum_simulator package: {{{ # cd into ros root dir roscd # clone repository git clone https://github.com/tum-vision/tum_simulator.git # add to ros path (if required) export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:`pwd`/tum_simulator # build package rosmake cvg_sim_gazebo_plugins rosmake message_to_tf }}} === Run Simulator === You can start the simulator with one of the following flying scenario: {{{ roslaunch cvg_sim_test 3boxes_room.launch }}} {{{ roslaunch cvg_sim_test outdoor_flight.launch }}} {{{ roslaunch cvg_sim_test tum_kitchen.launch }}} {{{ roslaunch cvg_sim_test tum_kitchen_with_marker.launch }}} {{{ roslaunch cvg_sim_test garching_kitchen.launch }}} {{{ roslaunch cvg_sim_test garching_kitchen_with_marker.launch }}} {{{ roslaunch cvg_sim_test wg_collada.launch }}} {{{ roslaunch cvg_sim_test competition.launch }}} === Manual control === ==== PS3 joystick control ==== You can manipulate the quadrocopter with joysticks after launching: {{{ roslaunch ardrone_joystick teleop.launch }}} * The L1 button starts the quadrocopter. It also works as a deadman button so that the robot will land if you release it during flight. * The left stick can be used to control the vx/vy-velocity. Keep in mind that these velocities are given in the local frame of the drone! * The right stick controls the yaw-rate and the altitude. * The select button can be used to switch between the two cameras. ==== terminal command control ==== * take off {{{ rostopic pub -1 /ardrone/takeoff std_msgs/Empty }}} * land {{{ rostopic pub -1 /ardrone/land std_msgs/Empty }}} * switch camera {{{ rosservice call /ardrone/togglecam }}} * motion {{{ # fly forward rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 1.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' # fly backward rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: -1.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' # fly to left rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 1.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' # fly to right rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: -1.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' # fly up rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 1.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' # fly down rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: -1.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' # counterclockwise rotation rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 1.0}}' # clockwise rotation rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: -1.0}}' # stop rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' }}} === Robot Sensor Monitoring === For observation you can use following commands: {{{ # The output camera rosrun image_view image_view image:=/ardrone/image_raw # The front camera rosrun image_view image_view image:=/ardrone/front/image_raw # The buttom camera rosrun image_view image_view image:=/ardrone/bottom/image_raw # The height sensor rostopic echo /sonar_height #The navigation info rostopic echo /ardrone/navdata #A launch file for joystick drivers and image view nodes roslaunch cvg_sim_test demo_tool.launch }}} OR using the rviz tool. == Troubleshooting == ===== Simulator starting error ===== If you get the following error message when you start the simulator, you should close all the other nodes which connect the simulator by some ROS topices. And try again to launch the simulator file. Furtherm more, you can terminate your ROS master, even close all your terminal windows and restart the simulator again. {{{ [gazebo-1] process has died [pid 1297, exit code 134, cmd /opt/ros/fuerte/stacks/simulator_gazebo/gazebo/scripts/gazebo /usr/stud/huangho/ros_workspace/cvpr-ros-internal/trunk/cvg_simulator/cvg_sim_gazebo/worlds/garching_kitchen.world __name:=gazebo __log:=/usr/stud/huangho/.ros/log/0a453de4-47a9-11e2-bff7-d4bed998ce90/gazebo-1.log]. log file: /usr/stud/huangho/.ros/log/0a453de4-47a9-11e2-bff7-d4bed998ce90/gazebo-1*.log }}} ===== Cannot control drone via joystick ===== * check, whether you are using the correct joystick * check, whether you joystick driver is running properly, and sending correct rostopics {{{ rostopic echo /joy }}} * check, whether the ardrone_joystick node is running properly by checking the following topics {{{ rostopic echo /cmd_vel rostopic echo /ardrone/land rostopic echo /ardrone/reset rostopic echo /ardrone/takeoff }}} ===== The simulated ardrone doing something oddly ===== * use rxgraph to see whether some other nodes send wrong command topics. ===== Cannot run the package (From Patrick Lehner) ===== - installing Gazebo 1.6.0 is not required (though its dependency libbullet might be), since launching gazebo via ROS will always use the Gazebo version of the ROS plugin (which is, for fuerte, 1.0.2) - Gazebo (and, by extension, the TUM simulator) is very picky about the installed graphics card; as far as we can tell at this time, only Nvidia cards really work (and only with the restricted Nvidia driver), ATI/AMD and Intel cards will not work, since the drivers do not have all the OpenGL/FireGL capabilities that Gazebo requires. In case of ATI drivers, we observed error messages about missing GL features, but with the Intel drivers, Gazebo simply segfaults (or rather, the Intel driver segfaults and Gazebo quits without any meaningful error message). - the ardrone_autonomy driver (i.e. the ardrone_driver executable) is not compatible with 64-bit systems (see https://projects.ardrone.org/issues/show/140 ) == Notes == Because of the limitation of the gazebo simulator, not all the AD.Drone sensors are 100% modeled in the simulation. Compared to the real AD.Drone ardrone_autonomy driver, the simulation can also send as the real AD.Drone with the same names: * Front camera information and image data * Bottom camera information and image data * Activated camera information and image data * IMU data * tf data * Navigation data The navigation data /ardrone/navdata includes many information. * message time stamp * message frame id * fly mode (partly) * battery percentage * position, rotation * velocity * acceleration are implemented. * magnet info * pressure * wind information * tags are not implemented. The following sensors are implemented in the simulation: * camera sensors * altitude sensor * IMU sensor == License == We release our code under the BSD license. However, as we strongly build upon Gazebo and HectorSLAM, corresponding licenses apply as well. == Contact == In case of problems, questions, or suggestions, please contact Hongrong Huang or Jürgen Sturm .