Initial commit

6065297c · Pateman16 · 6065297c · 6065297c · 6065297c · 6065297c
Commit 6065297c authored 7 years ago by Pateman16
--- a/.gitignore
+++ b/.gitignore
+devel/
+logs/
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+cmake_minimum_required(VERSION 2.8.3)
+project(simulation_control)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  actionlib
+  actionlib_msgs
+  mavros
+  message_generation
+  roscpp
+  rospy
+  std_msgs
+  geometry_msgs
+  apriltags2_ros
+
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+add_action_files(
+  FILES
+  goto_position.action
+  detect_object.action
+  center_on_object.action
+  descend_on_object.action
+ )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  actionlib_msgs
+  std_msgs
+  geometry_msgs
+  apriltags2_ros
+ )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES simulation_control
+  CATKIN_DEPENDS actionlib actionlib_msgs mavros message_generation roscpp rospy std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/simulation_control.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/simulation_control_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_simulation_control.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- a/README
+++ b/README
+If you have the enviroment set up as the tutorial found here https://drive.google.com/open?id=1TP-kR2L8zDoeb2tTdHy4Co4Aqj2QQSS0wQGtv7M0Dlo
+this is what you got to do
+
+open three new terminals
+
+terminal1: roslaunch simulation_control posix_sitl.launch
+
+wait a little while
+
+terminal2: roslaunch simulation_control px4.launch fcu_url:="udp://:14550@127.0.0.1:14557"
+
+terminal3: roslaunch simulation_control simulation_control.launch 
--- a/action/center_on_object.action
+++ b/action/center_on_object.action
+#goal definition
+std_msgs/String data
+---
+#result definition
+std_msgs/Bool centered
+---
+#feedback
+geometry_msgs/PoseStamped current_position
--- a/action/descend_on_object.action
+++ b/action/descend_on_object.action
+#goal definition
+std_msgs/Float32 height
+---
+#result definition
+std_msgs/Bool position_reached
+---
+#feedback
+geometry_msgs/PoseStamped current_position
--- a/action/detect_object.action
+++ b/action/detect_object.action
+#goal definition
+std_msgs/String data
+---
+#result definition
+geometry_msgs/PoseStamped detected_position
+---
+#feedback
+geometry_msgs/PoseStamped current_position
--- a/action/goto_position.action
+++ b/action/goto_position.action
+#goal definition
+geometry_msgs/PoseStamped destination
+---
+#result definition
+std_msgs/String data
+---
+#feedback
+geometry_msgs/PoseStamped current_position
--- a/package.xml
+++ b/package.xml
+<?xml version="1.0"?>
+<package format="2">
+  <name>simulation_control</name>
+  <version>0.0.0</version>
+  <description>The simulation_control package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="pateman@todo.todo">pateman</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/simulation_control</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>actionlib</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+  <build_depend>mavros</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>actionlib</build_export_depend>
+  <build_export_depend>actionlib_msgs</build_export_depend>
+  <build_export_depend>mavros</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>actionlib</exec_depend>
+  <exec_depend>actionlib_msgs</exec_depend>
+  <exec_depend>mavros</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_generation</exec_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/src/Startup.txt
+++ b/src/Startup.txt
+If you have the enviroment set up as the tutorial found here https://drive.google.com/open?id=1TP-kR2L8zDoeb2tTdHy4Co4Aqj2QQSS0wQGtv7M0Dlo
+this is what you got to do
+
+open three new terminals
+
+terminal1: roslaunch simulation_control posix_sitl.launch
+
+wait a little while
+
+terminal2: roslaunch simulation_control px4.launch fcu_url:="udp://:14550@127.0.0.1:14557"
+
+terminal3: roslaunch simulation_control simulation_control.launch 
--- a/src/launch/posix_sitl.launch
+++ b/src/launch/posix_sitl.launch
+<launch>
+
+    <!-- Posix SITL environment launch script -->
+    <arg name="x" default="0"/>
+    <arg name="y" default="25"/>
+    <arg name="z" default="0"/>
+    <arg name="R" default="0"/>
+    <arg name="P" default="0"/>
+    <arg name="Y" default="0"/>
+
+    <arg name="est" default="lpe"/>
+    <arg name="vehicle" default="f450"/>
+    <arg name="world" default="$(find simulation_control)/worlds/crazy_bluebox.world"/>
+    <arg name="sdf" default="$(find simulation_control)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
+    <arg name="rcS" default="$(find px4)/posix-configs/SITL/init/$(arg est)/$(arg vehicle)"/>
+
+    <arg name="headless" default="false"/>
+    <arg name="gui" default="true"/>
+    <arg name="ns" default="/"/>
+
+    <arg name="debug" default="false"/>
+    <arg name="verbose" default="false"/>
+    <arg name="paused" default="false"/>
+
+    <node name="sitl" pkg="px4" type="px4" output="screen"
+        args="$(find px4) $(arg rcS)">
+    </node>
+
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <arg name="headless" value="$(arg headless)"/>
+        <arg name="gui" value="$(arg gui)"/>
+        <arg name="world_name" value="$(arg world)" />
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="verbose" value="$(arg verbose)" />
+        <arg name="paused" value="$(arg paused)" />
+    </include>
+
+    <node name="$(anon vehicle_spawn)" output="screen" pkg="gazebo_ros" type="spawn_model"
+        args="-sdf -file $(arg sdf) -model $(arg vehicle) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
+
+
+</launch>
+
+<!-- vim: set et ft=xml fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : -->
--- a/src/launch/px4.launch
+++ b/src/launch/px4.launch
+<launch>
+	<!-- vim: set ft=xml noet : -->
+	<!-- example launch script for PX4 based FCU's -->
+
+	<arg name="fcu_url" default="/dev/ttyACM0:57600" />
+	<arg name="gcs_url" default="" />
+	<arg name="tgt_system" default="1" />
+	<arg name="tgt_component" default="1" />
+	<arg name="log_output" default="screen" />
+	<arg name="fcu_protocol" default="v2.0" />
+
+	<include file="$(find mavros)/launch/node.launch">
+		<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
+		<arg name="tgt_system" value="$(arg tgt_system)" />
+		<arg name="tgt_component" value="$(arg tgt_component)" />
+		<arg name="log_output" value="$(arg log_output)" />
+		<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
+	</include>
+</launch>
--- a/src/launch/simulation_control.launch
+++ b/src/launch/simulation_control.launch
+<launch>
+  <node name="position_control" pkg="simulation_control" type="position_control.py" output="screen" />
+  <node name="action_controller" pkg="simulation_control" type="action_controller.py" output="screen" />
+  <node name="goto_position_server" pkg="simulation_control" type="goto_position_server.py" output="screen" />
+  <node name="detect_object_server" pkg="simulation_control" type="detect_object_server.py" output="screen" />
+  <node name="color_detection" pkg="simulation_control" type="color_detection.py" output="screen" />
+  <node name="descend_on_object_server" pkg="simulation_control" type="descend_on_object_server.py" output="screen" />
+</launch>
--- a/src/models/blue_box_mark/model.config
+++ b/src/models/blue_box_mark/model.config
+<?xml version="1.0" ?>
+<model>
+    <name>blue_box_mark</name>
+    <version>1.0</version>
+    <sdf version="1.6">model.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>
--- a/src/models/blue_box_mark/model.sdf
+++ b/src/models/blue_box_mark/model.sdf
+<?xml version='1.0'?>
+<sdf version='1.6'>
+  <model name='blue_box_mark'>
+    <link name='link'>
+      <pose frame=''>0 0 -0.120735 0 -0 0</pose>
+      <inertial>
+        <mass>1</mass>
+        <inertia>
+          <ixx>0.166667</ixx>
+          <ixy>0</ixy>
+          <ixz>0</ixz>
+          <iyy>0.166667</iyy>
+          <iyz>0</iyz>
+          <izz>0.166667</izz>
+        </inertia>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+      </inertial>
+      <self_collide>0</self_collide>
+      <kinematic>0</kinematic>
+      <gravity>1</gravity>
+      <visual name='visual'>
+        <geometry>
+          <box>
+            <size>0.5 0.5 0.01</size>
+          </box>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Blue</name>
+            <uri>__default__</uri>
+          </script>
+          <ambient>0 0 1 1</ambient>
+          <diffuse>0 0 1 1</diffuse>
+          <specular>0.1 0.1 0.1 1</specular>
+          <emissive>0 0 0 1</emissive>
+          <shader type='vertex'>
+            <normal_map>__default__</normal_map>
+          </shader>
+        </material>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <transparency>0</transparency>
+        <cast_shadows>1</cast_shadows>
+      </visual>
+      <collision name='collision'>
+        <laser_retro>0</laser_retro>
+        <max_contacts>10</max_contacts>
+        <pose frame=''>0 0 0 0 -0 0</pose>
+        <geometry>
+          <box>
+            <size>0.5 0.5 0.01</size>
+          </box>
+        </geometry>
+        <surface>
+          <friction>
+            <ode>
+              <mu>1</mu>
+              <mu2>1</mu2>
+              <fdir1>0 0 0</fdir1>
+              <slip1>0</slip1>
+              <slip2>0</slip2>
+            </ode>
+            <torsional>
+              <coefficient>1</coefficient>
+              <patch_radius>0</patch_radius>
+              <surface_radius>0</surface_radius>
+              <use_patch_radius>1</use_patch_radius>
+              <ode>
+                <slip>0</slip>
+              </ode>
+            </torsional>
+          </friction>
+          <bounce>
+            <restitution_coefficient>0</restitution_coefficient>
+            <threshold>1e+06</threshold>
+          </bounce>
+          <contact>
+            <collide_without_contact>0</collide_without_contact>
+            <collide_without_contact_bitmask>1</collide_without_contact_bitmask>
+            <collide_bitmask>1</collide_bitmask>
+            <ode>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+              <max_vel>0.01</max_vel>
+              <min_depth>0</min_depth>
+            </ode>
+            <bullet>
+              <split_impulse>1</split_impulse>
+              <split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
+              <soft_cfm>0</soft_cfm>
+              <soft_erp>0.2</soft_erp>
+              <kp>1e+13</kp>
+              <kd>1</kd>
+            </bullet>
+          </contact>
+        </surface>
+      </collision>
+    </link>
+    <static>0</static>
+    <allow_auto_disable>1</allow_auto_disable>
+  </model>
+</sdf>
--- a/src/models/f450/f450.sdf
+++ b/src/models/f450/f450.sdf
--- a/src/models/f450/meshes/f450.dae
+++ b/src/models/f450/meshes/f450.dae
--- a/src/models/f450/meshes/propeller_ccw.dae
+++ b/src/models/f450/meshes/propeller_ccw.dae
--- a/src/models/f450/meshes/propeller_cw.dae
+++ b/src/models/f450/meshes/propeller_cw.dae
+<?xml version="1.0" encoding="UTF-8"?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+    <asset>
+        <contributor>
+            <author>VCGLab</author>
+            <authoring_tool>VCGLib | MeshLab</authoring_tool>
+        </contributor>
+        <up_axis>Y_UP</up_axis>
+        <created>Fr. Dez 19 16:04:46 2014</created>
+        <modified>Fr. Dez 19 16:04:46 2014</modified>
+    </asset>
+    <library_images/>
+    <library_materials/>
+    <library_effects/>
+    <library_geometries>
+        <geometry id="shape0-lib" name="shape0">
+            <mesh>
+                <source id="shape0-lib-positions" name="position">
+                    <float_array id="shape0-lib-positions-array" count="300">-0.0835714 0.140031 -0.0736456 -0.0400765 0.163115 -0.0483281 -0.117913 0.305442 -0.144312 -0.0703607 0.798102 -0.0199993 -0.0560492 0.545571 -0.0512484 -0.00078476 0.975427 0.00767503 0.0160651 0.594631 -0.00720468 0.0298948 0.965824 0.00334253 0.00751962 0.685387 0.000359752 0.0540712 0.429634 -0.0101518 0.0151478 0.102216 -0.0131658 -0.0556911 -0.0661715 -0.00963317 -0.00174063 -0.0818349 -0.00868353 0.0559242 -0.0649661 -0.00957289 0.0703602 -0.798109 -0.0199979 0.0560498 -0.54557 -0.0512488 0.0750017 -0.975039 -0.00934454 0.000782845 -0.975427 0.00767481 -0.01485 -0.614361 -0.00403874 -0.0298926 -0.965824 0.00334368 -0.0548356 -0.290864 -0.0115746 0.0115154 -0.203636 -0.0264133 0.118578 -0.302999 -0.139409 -0.0121224 -0.120255 -0.0720243 0.0226319 -0.110745 -0.133017 -0.00674525 0.0938006 -0.100237 -0.0231875 -0.121853 -0.0140071 -0.0227039 -0.852975 0.00390995 0.0723746 -0.204958 -0.0813719 0.0406506 -0.549299 -0.0369143 -0.0549052 -0.803689 -0.00777506 -0.0303961 -0.366957 -0.0129591 0.00129643 -0.676714 -0.0029672 0.0229853 0.118236 -0.0121112 -0.0122597 0.134668 -0.0463299 -0.00497452 0.0848861 -0.010146 -0.026551 0.101783 -0.0245763 0.0142935 0.08864 -0.0122476 0.00830604 -0.0268358 -0.0117744 0.00449618 -0.0836288 -0.0193353 0.022756 -0.115931 -0.0296084 -0.00274884 0.0269521 -0.0451194 0.0193773 0.0148156 -0.0105058 -0.0177061 -0.00590916 -0.0110817 -0.00867444 -0.0773042 -0.00921494 -0.0148637 -0.0936643 -0.0989573 -0.00392095 0.206809 -0.0239818 -0.0750017 0.975039 -0.00934454 0.0227405 0.85053 0.00378915 0.0574457 0.791463 -0.00951397 0.000301326 0.377456 -0.019279 -0.025568 0.993204 0.00603233 0.0165912 0.104876 -0.0548642 0.0173898 0.0893329 -0.135902 -0.07848 0.839861 -0.0171015 -0.0410062 0.563133 -0.0362053 -0.104112 0.546217 -0.0949896 0.053552 0.950918 -0.00758994 0.00787323 0.603278 -0.00724069 -0.11474 0.256342 -0.121651 -0.118667 0.439386 -0.118333 0.0562149 0.618315 0.000361391 0.0297782 0.463137 -0.00252085 0.0303766 0.95587 0.0126425 0.0573177 0.951992 -4.06283e-05 0.0582776 0.0699707 -0.000511708 -0.0558582 0.066586 -0.00972389 -0.118004 0.364506 -0.138633 -0.0937259 0.247795 -0.109201 -0.0129714 -0.0875212 -0.000267106 -0.0142037 0.0889641 -0.00118289 -0.058478 0.0641806 -4.29309e-06 0.0787758 -0.813625 -0.0191612 0.118975 -0.372814 -0.139636 0.0368024 -0.992893 0.0026906 -0.0533484 -0.950983 -0.00752904 0.083117 -0.138354 -0.0731762 0.0818296 -0.808715 -0.0116048 -0.0573177 -0.951992 -4.06283e-05 -0.0298975 -0.956894 0.0126223 0.0582653 -0.0732486 -0.00292781 0.112117 -0.483966 -0.11112 -0.0583462 -0.188043 -0.00926188 -0.0578409 -0.0756444 -0.000201417 0.0103472 -0.4132 -0.014694 0.0408722 -1.00118 0.0125076 0.118766 -0.442389 -0.115879 0.0609817 -0.51919 -0.0445129 0.123134 -0.278077 -0.131944 0.0370371 -0.13461 -0.0302756 -0.0511726 -0.503887 -0.000242579 -0.00561221 -0.652544 0.00854968 0.00138781 -0.137464 -0.0204127 0.00564852 0.249971 -0.0118047 0.00393641 0.767754 0.0145574 -0.0408722 1.00118 0.0125076 -0.0603939 0.509636 -0.0446996 -0.0514677 0.26177 -0.0479069 -0.0805473 0.828811 -0.0103958 0.0161138 -0.103875 -0.0375713</float_array>
+                    <technique_common>
+                        <accessor count="100" source="#shape0-lib-positions-array" stride="3">
+                            <param name="X" type="float"/>
+                            <param name="Y" type="float"/>
+                            <param name="Z" type="float"/>
+                        </accessor>
+                    </technique_common>
+                </source>
+                <source id="shape0-lib-normals" name="normal">
+                    <float_array id="shape0-lib-normals-array" count="597">-0.133297 0.0211742 -0.99085 0.574718 -0.217697 -0.788864 0.458684 0.116613 -0.880915 0.796763 0.0286666 -0.603612 0.637445 0.1301 -0.759433 0.428682 0.134839 -0.893336 0.274549 0.064973 -0.959376 0.0857926 0.0275753 -0.995931 -0.0247788 0.0121475 -0.999619 0.430487 0.115151 -0.895221 -0.25635 0.0307289 -0.966095 -0.201373 0.00360488 -0.979508 0.0843124 -0.000850521 -0.996439 0.549969 0.0165927 -0.83502 0.516535 0.00940306 -0.856215 0.322449 0.052078 -0.945153 -0.0335466 -0.0296519 -0.998997 0.607426 -0.378598 -0.698353 0.15074 -0.388236 -0.909148 0.000547457 -0.000682649 -1 -0.188206 0.602477 -0.775629 0.00115012 -0.0565694 -0.998398 -0.580676 0.05504 -0.812272 0.131633 -0.0264486 -0.990946 -0.639939 0.164155 -0.750687 -0.600964 -0.131612 -0.788366 -0.455317 -0.134708 -0.88008 -0.272125 -0.0649464 -0.960068 -0.155379 -0.0387502 -0.987095 -0.16364 -0.0431855 -0.985574 0.234548 -0.0304476 -0.971627 0.127333 -0.00736573 -0.991833 0.020128 0.00871479 -0.999759 -0.33958 0.0991923 -0.935332 -0.746178 -0.0489602 -0.663944 -0.547152 -0.0104714 -0.836968 -0.303996 -0.0583859 -0.950883 -0.411912 0.906427 -0.0933727 -0.383743 -0.922565 0.0401973 0.568828 0.682376 -0.459127 0.42064 -0.738002 -0.527651 -0.00175281 0.0309075 -0.999521 -0.536497 0.180328 -0.82441 0.701499 -0.711733 -0.0365385 0.0317392 0.0966715 -0.99481 -0.95107 0.308836 0.00929138 -0.669577 -0.0341059 -0.741959 -0.789851 -0.00640954 -0.613265 -0.399725 -0.118701 -0.908917 -0.277976 -0.051835 -0.959188 -0.0846766 -0.0273322 -0.996034 -0.493359 -0.0784218 -0.866284 0.319854 -0.0156218 -0.947338 0.422089 0.002947 -0.90655 0.242721 -0.0707989 -0.967509 0.127605 -0.0189321 -0.991644 -0.151454 -0.00323371 -0.988459 -0.191947 -0.0052902 -0.981391 0.315678 0.110243 0.94244 -0.296303 0.0100185 -0.955041 -0.287174 -0.0493665 -0.956605 0.959209 -0.281808 0.0224395 0.191154 -0.98156 -0.000713827 -0.556961 -0.820605 -0.128067 -0.030548 -0.025678 -0.999203 0.286646 -0.565131 -0.773603 0.173521 -0.502022 -0.847269 0.873465 0.486328 0.0233435 0.0581331 0.332251 -0.941398 -0.355835 0.533283 -0.767458 -0.528274 0.187115 -0.828199 0.0581217 0.0149521 -0.998198 0.142486 0.121223 -0.982346 -0.0126377 0.0120754 -0.999847 -0.03695 -0.000729376 -0.999317 0.00278682 -0.0257849 -0.999664 0.301704 0.934056 -0.191087 0.26447 0.94675 -0.183633 -0.930448 0.202214 -0.305576 0.441304 0.0138075 -0.897251 0.55055 -0.214323 -0.806821 0.28063 0.0477879 -0.958626 -0.33254 0.0169824 -0.942936 -0.41912 0.000720725 -0.907931 -0.268139 0.0590185 -0.961571 -0.273083 0.0603385 -0.960096 0.0389661 -0.00683833 -0.999217 0.720886 0.127695 -0.681188 0.689043 0.002923 -0.724715 0.648741 -0.135545 -0.748841 0.636136 0.0714873 -0.768258 0.491251 0.0299339 -0.870504 0.0722015 0.0401157 -0.996583 0.261315 0.0552144 -0.963673 -0.93967 -0.286378 -0.187102 0.320575 -0.946574 0.0350669 -0.56586 -0.771852 -0.289907 0.63497 0.705242 -0.315353 -0.935602 0.228627 -0.269033 -0.0473885 -0.994583 0.0925202 -0.0974954 0.977242 -0.188393 -0.596161 0.787386 -0.156894 -0.871918 0.0175052 -0.489339 -0.965453 -0.00139316 -0.260572 0.511024 0.229124 -0.828466 0.654563 0.139928 -0.742945 0.431224 0.120127 -0.894212 0.790089 0.00978929 -0.612914 0.669775 0.0702733 -0.739231 -0.547135 0.0147329 0.836915 -0.97102 0.0385212 -0.235874 0.00566892 0.637056 -0.770796 0.163542 0.0708451 -0.983989 -0.991119 0.0619576 0.117662 -0.147734 -0.0402535 -0.988208 0.170092 -0.000234405 -0.985428 0.973803 -0.00171444 -0.227389 -0.484979 0.870415 0.0846928 -0.377604 0.380711 0.844082 -0.815008 0.22749 -0.532926 0.468273 -0.123234 -0.874948 -0.959682 -0.162511 0.229347 0.416634 -0.24919 -0.874254 -0.998513 -0.0223246 0.0497307 -0.720235 -0.118933 0.683459 0.00420263 0.00958784 0.999945 0.40185 0.635479 0.659305 0.394973 0.861298 -0.319628 0.892965 0.0272271 -0.449302 0.425897 -0.000317519 0.904772 -0.0970638 -0.00194985 0.995276 -0.00807406 0.00452466 0.999957 0.999987 0.00109094 -0.00496858 0.368027 -9.60815e-05 0.929815 0.209665 -0.019518 0.977579 -0.9868 0.116086 -0.112916 0.522797 0.822842 0.222743 0.0406913 -0.0168571 0.99903 -0.00791449 0.00513354 0.999956 0.0236165 0.00535241 0.999707 0.00109216 -0.00140481 0.999998 0.571992 -0.490798 0.657223 0.949872 0.308613 0.0500148 0.945265 0.00542256 -0.326259 -0.543624 0.188326 -0.817928 -0.640792 -0.153441 -0.752224 0.947093 -0.171471 -0.271317 -0.646631 -0.143584 -0.749168 0.54514 -0.0109876 0.838273 0.935718 -0.0431707 -0.350098 0.13002 -0.730264 -0.670679 -0.183519 -0.069504 -0.980556 0.996045 -0.0637611 0.0618805 -0.132039 -0.000143242 -0.991244 0.671823 -0.115333 0.731678 0.697289 0.130325 0.704843 0.928302 -0.185352 -0.322336 0.00951954 0.0126202 0.999875 -0.409606 -0.657436 0.632456 -0.393671 -0.860025 -0.324624 0.0186685 0.000194589 0.999826 -0.998019 -0.000584304 0.0629117 -0.915486 -0.00608856 -0.402304 -0.418349 0.00614631 0.908266 0.807142 -0.192938 -0.55794 0.99471 -0.0358545 -0.096262 -0.634721 0.0901806 -0.767461 -0.475878 -0.759235 -0.443962 -0.88294 -0.010116 -0.469376 -0.993521 -0.00468689 0.113555 0.121164 -0.438756 0.8904 -0.189839 0.502055 -0.843743 0.075519 0.126612 -0.989074 -0.636105 -0.100472 -0.765034 -0.948654 0.135966 -0.285602 0.848919 0.0460372 -0.526514 -0.0104485 -0.0449987 0.998932 0.223937 0.0033933 0.974598 -0.11762 0.0226705 0.9928 0.312214 0.0523961 0.948566 0.441463 0.132655 0.887419 0.75261 0.044227 0.656979 0.780304 0.0254496 0.624882 0.51369 -0.00122638 0.857975 0.506904 -0.000351007 0.862003 0.100823 0.0895047 0.99087 0.570298 0.0416434 0.820382 -0.989057 0.115932 -0.0912446 0.145214 -0.0430987 0.988461 -0.317694 -0.0526722 0.946729 -0.770633 -0.028076 0.636661 -0.757154 -0.0357068 0.65226 -0.512754 0.119312 0.850205 -0.759325 0.0121701 0.650598 -0.538516 -0.0302892 0.84207 0.062979 -0.0505388 0.996734 -0.577001 -0.0434322 0.815588 -0.361324 -0.122166 0.924403 0.794431 -0.38983 -0.465738</float_array>
+                    <technique_common>
+                        <accessor count="199" source="#shape0-lib-normals-array" stride="3">
+                            <param name="X" type="float"/>
+                            <param name="Y" type="float"/>
+                            <param name="Z" type="float"/>
+                        </accessor>
+                    </technique_common>
+                </source>
+                <vertices id="shape0-lib-vertices">
+                    <input semantic="POSITION" source="#shape0-lib-positions"/>
+                </vertices>
+                <triangles count="199">
+                    <input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
+                    <input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
+                    <p>7 0 8 0 5 0 1 1 0 1 2 1 4 2 8 2 6 2 1 3 2 3 4 3 4 4 2 4 3 4 8 5 4 5 3 5 51 6 3 6 47 6 51 7 5 7 8 7 10 8 6 8 9 8 6 9 8 9 9 9 7 10 49 10 8 10 49 11 9 11 8 11 13 12 10 12 9 12 10 13 1 13 4 13 10 14 4 14 6 14 8 15 3 15 51 15 13 16 66 16 10 16 1 17 66 17 0 17 1 18 10 18 66 18 11 19 66 19 13 19 76 20 12 20 13 20 11 21 13 21 12 21 12 22 76 22 21 22 19 23 18 23 17 23 21 24 76 24 22 24 15 25 22 25 14 25 18 26 15 26 14 26 74 27 14 27 16 27 74 28 17 28 18 28 21 29 18 29 20 29 19 30 30 30 18 30 30 31 20 31 18 31 20 32 11 32 12 32 21 33 20 33 12 33 21 34 22 34 15 34 21 35 15 35 18 35 18 36 14 36 74 36 24 37 23 37 99 37 24 38 99 38 45 38 34 39 52 39 25 39 43 40 41 40 42 40 38 41 43 41 42 41 40 42 28 42 92 42 92 43 24 43 40 43 26 44 11 44 44 44 44 45 45 45 26 45 29 46 92 46 28 46 29 47 28 47 73 47 29 48 72 48 32 48 16 49 74 49 72 49 32 50 74 50 27 50 29 51 32 51 31 51 27 52 19 52 30 52 19 53 75 53 30 53 27 54 30 54 32 54 32 55 30 55 31 55 31 56 30 56 82 56 31 57 11 57 92 57 92 58 11 58 26 58 92 59 29 59 31 59 74 60 32 60 72 60 33 61 37 61 53 61 53 62 37 62 35 62 53 63 35 63 36 63 37 64 13 64 42 64 0 65 36 65 66 65 66 66 36 66 35 66 24 67 39 67 40 67 13 68 40 68 39 68 40 69 13 69 76 69 76 70 28 70 40 70 42 71 13 71 38 71 13 72 39 72 38 72 35 73 43 73 66 73 66 74 43 74 11 74 11 75 43 75 44 75 45 76 44 76 39 76 45 77 39 77 24 77 46 78 34 78 36 78 46 79 50 79 33 79 68 80 36 80 0 80 54 81 47 81 51 81 7 82 49 82 48 82 49 83 7 83 57 83 48 84 49 84 58 84 58 85 49 85 50 85 37 86 33 86 13 86 33 87 34 87 46 87 68 88 50 88 46 88 36 89 68 89 46 89 50 90 68 90 55 90 50 91 55 91 58 91 58 92 51 92 48 92 58 93 54 93 51 93 45 94 23 94 26 94 26 95 23 95 24 95 26 96 24 96 92 96 33 97 52 97 34 97 34 98 25 98 36 98 36 99 25 99 53 99 53 100 25 100 52 100 53 101 52 101 33 101 71 102 11 102 82 102 71 103 66 103 11 103 60 104 55 104 67 104 54 105 55 105 56 105 54 106 58 106 55 106 55 107 68 107 67 107 55 108 60 108 56 108 47 109 98 109 95 109 54 110 98 110 47 110 47 111 95 111 51 111 51 112 63 112 48 112 48 113 63 113 7 113 33 114 50 114 13 114 50 115 49 115 13 115 65 116 13 116 49 116 71 117 70 117 66 117 59 118 66 118 70 118 60 119 98 119 56 119 59 120 67 120 68 120 66 121 59 121 0 121 0 122 59 122 68 122 67 123 59 123 60 123 60 124 96 124 98 124 94 125 63 125 95 125 63 126 64 126 7 126 57 127 7 127 64 127 49 128 57 128 64 128 63 129 61 129 64 129 62 130 65 130 61 130 62 131 70 131 65 131 65 132 49 132 64 132 61 133 65 133 64 133 63 134 94 134 61 134 56 135 98 135 54 135 95 136 63 136 51 136 80 137 65 137 69 137 69 138 65 138 70 138 69 139 70 139 71 139 69 140 71 140 83 140 80 141 89 141 76 141 80 142 76 142 13 142 13 143 65 143 80 143 76 144 88 144 28 144 81 145 29 145 73 145 77 146 72 146 81 146 72 147 29 147 81 147 16 148 77 148 85 148 72 149 77 149 16 149 16 150 85 150 74 150 74 151 79 151 27 151 27 152 79 152 19 152 11 153 31 153 82 153 76 154 89 154 88 154 77 155 86 155 87 155 81 156 86 156 77 156 91 157 79 157 85 157 79 158 78 158 19 158 75 159 19 159 78 159 78 160 90 160 83 160 83 161 82 161 78 161 82 162 30 162 78 162 79 163 90 163 78 163 86 164 81 164 73 164 86 165 73 165 88 165 73 166 28 166 88 166 85 167 79 167 74 167 78 168 30 168 75 168 82 169 83 169 71 169 89 170 80 170 69 170 35 171 37 171 41 171 38 172 39 172 43 172 43 173 39 173 44 173 41 174 43 174 35 174 41 175 37 175 42 175 69 176 83 176 84 176 84 177 83 177 90 177 91 178 90 178 79 178 85 179 77 179 91 179 77 180 87 180 91 180 87 181 86 181 88 181 87 182 88 182 89 182 69 183 84 183 89 183 89 184 84 184 87 184 84 185 90 185 91 185 87 186 84 186 91 186 70 187 62 187 93 187 62 188 61 188 94 188 95 189 98 189 94 189 96 190 60 190 59 190 96 191 59 191 97 191 70 192 93 192 97 192 59 193 70 193 97 193 97 194 93 194 96 194 93 195 62 195 94 195 96 196 93 196 94 196 96 197 94 197 98 197 45 198 99 198 23 198</p>
+                </triangles>
+            </mesh>
+        </geometry>
+    </library_geometries>
+    <library_visual_scenes>
+        <visual_scene id="VisualSceneNode" name="VisualScene">
+            <node id="node" name="node">
+                <instance_geometry url="#shape0-lib">
+                    <bind_material>
+                        <technique_common/>
+                    </bind_material>
+                </instance_geometry>
+            </node>
+        </visual_scene>
+    </library_visual_scenes>
+    <scene>
+        <instance_visual_scene url="#VisualSceneNode"/>
+    </scene>
+</COLLADA>
--- a/src/models/f450/model.config
+++ b/src/models/f450/model.config
+<?xml version="1.0" ?>
+<model>
+    <name>UPenn F450</name>
+    <version>1.0</version>
+    <sdf version="1.5">f450.sdf</sdf>
+    <author>
+        <name></name>
+        <email></email>
+    </author>
+    <description></description>
+</model>
--- a/src/scripts/PID.py
+++ b/src/scripts/PID.py
+#!/usr/bin/env python
+import rospy
+
+class PID:
+    def __init__(self, Kp=0.2, Ki=0.001, Kd=0.001, maxI=0.1, maxOut=3.0):
+        self.Kp = Kp
+        self.Ki = Ki
+        self.Kd = Kd
+
+        self.target = 0
+        self.output = 0
+        self.error = 0
+        self.maxI = maxI
+        self.maxOut = maxOut
+        self.reset()
+        self.lastTime = rospy.get_time()
+
+    def update(self, target, state, time):
+        # u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt}
+        self.target = target
+        self.state = state
+        self.error = self.target - self.state
+
+        self.time = time
+        dTime = self.time - self.lastTime
+        dError = self.error - self.lastError
+        #print("DTime: {}, dError: {}".format(dTime, dError))
+
+        p = self.error
+        self.intError += self.error * dTime
+        if (dTime > 0):
+            d = dError / dTime
+        else:
+            d = 0
+            rospy.logdebug("Change in time is zero.")
+        # Make sure I does not exceed maximum windup
+        if (self.maxI is not None and self.intError > self.maxI):
+            i = self.maxI
+        elif (self.maxI is not None and self.intError < -self.maxI):
+            i = self.maxI
+        else:
+            i = self.intError
+
+        # Remember last time and last error for next calculation
+        self.lastTime = self.time
+        self.lastError = self.lastError
+
+        output = self.Kp * p + self.Ki * i + self.Kd * d
+        # Make sure output does not exceed maximum
+        if (self.maxOut is not None and output > self.maxOut):
+            output = self.maxOut
+        elif (self.maxOut is not None and output < -self.maxOut):
+            output = -self.maxOut
+
+        return output
+
+    def setKp(self, Kp):
+        self.Kp = Kp
+
+    def setKi(self, Ki):
+        self.Ki = Ki
+
+    def setKd(self, Kd):
+        self.Kd = Kd
+
+    def setMaxI(self, maxI):
+        self.maxI = maxI
+    def setMaxO(self, MaxO):
+        self.maxOut = MaxO
+
+    def reset(self):
+        self.target = 0.0
+        self.error = 0.0
+        self.state = 0.0
+        self.intError = 0.0
+        self.lastError = 0.0
+        self.output = 0.0
\ No newline at end of file