diff --git a/f550_amazing b/f550_amazing
new file mode 100644
index 0000000000000000000000000000000000000000..bdb77afb53755a501e84c68de84d4d969b76b7d6
--- /dev/null
+++ b/f550_amazing
@@ -0,0 +1,79 @@
+uorb start
+param load
+param set MAV_TYPE 13
+param set SYS_AUTOSTART 6001
+param set SYS_RESTART_TYPE 2
+dataman start
+param set BAT_N_CELLS 3
+param set CAL_GYRO0_ID 2293768
+param set CAL_ACC0_ID 1376264
+param set CAL_ACC1_ID 1310728
+param set CAL_MAG0_ID 196616
+param set CAL_GYRO0_XOFF 0.00
+param set CAL_ACC0_XOFF 0.00
+param set CAL_ACC0_YOFF -0.00
+param set CAL_ACC0_ZOFF 0.00
+param set CAL_ACC0_XSCALE 1.00
+param set CAL_ACC0_YSCALE 1.00
+param set CAL_ACC0_ZSCALE 1.00
+param set CAL_ACC1_XOFF 0.00
+param set CAL_MAG0_XOFF 0.00
+param set SENS_BOARD_ROT 0
+param set SENS_BOARD_X_OFF 0.000000
+param set COM_RC_IN_MODE 1
+param set NAV_DLL_ACT 2
+param set COM_DISARM_LAND 3
+param set NAV_ACC_RAD 2.0
+param set RTL_RETURN_ALT 30.0
+param set RTL_DESCEND_ALT 10.0
+param set RTL_LAND_DELAY 0
+param set MIS_TAKEOFF_ALT 2.5
+param set MC_ROLLRATE_P 0.2
+param set MC_PITCHRATE_P 0.2
+param set MC_PITCH_P 6
+param set MC_ROLL_P 6
+param set MPC_HOLD_MAX_Z 2.0
+param set MPC_HOLD_XY_DZ 0.1
+param set MPC_Z_VEL_P 0.6
+param set MPC_Z_VEL_I 0.15
+param set MPC_XY_VEL_P 0.15
+param set MPC_XY_VEL_I 0.2
+param set EKF2_GBIAS_INIT 0.0
+param set EKF2_ANGERR_INIT 0.0
+param set EKF2_MAG_TYPE 1
+replay tryapplyparams
+simulator start -s
+rgbledsim start
+tone_alarm start
+gyrosim start
+accelsim start
+barosim start
+adcsim start
+gpssim start
+pwm_out_sim mode_pwm16
+sleep 1
+sensors start
+commander start
+land_detector start multicopter
+navigator start
+attitude_estimator_q start
+local_position_estimator start
+mc_pos_control start
+mc_att_control start
+mixer load /dev/pwm_output0 ROMFS/px4fmu_common/mixers/hexa_x.main.mix
+mavlink start -u 14556 -r 4000000
+mavlink start -u 14557 -r 4000000 -m onboard -o 14540
+mavlink stream -r 50 -s POSITION_TARGET_LOCAL_NED -u 14556
+mavlink stream -r 50 -s LOCAL_POSITION_NED -u 14556
+mavlink stream -r 50 -s GLOBAL_POSITION_INT -u 14556
+mavlink stream -r 50 -s ATTITUDE -u 14556
+mavlink stream -r 50 -s ATTITUDE_QUATERNION -u 14556
+mavlink stream -r 50 -s ATTITUDE_TARGET -u 14556
+mavlink stream -r 50 -s SERVO_OUTPUT_RAW_0 -u 14556
+mavlink stream -r 20 -s RC_CHANNELS -u 14556
+mavlink stream -r 250 -s HIGHRES_IMU -u 14556
+mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
+sdlog2 start -r 100 -e -t -a
+vmount start
+mavlink boot_complete
+replay trystart
diff --git a/gripper_plugin.cc b/gripper_plugin.cc
new file mode 100644
index 0000000000000000000000000000000000000000..839d1a1ff1ba32c9395c057e36b099e48a5c4332
--- /dev/null
+++ b/gripper_plugin.cc
@@ -0,0 +1,147 @@
+#ifndef _VELODYNE_PLUGIN_HH_
+#define _VELODYNE_PLUGIN_HH_
+
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+
+#include <thread>
+#include "ros/ros.h"
+#include "ros/callback_queue.h"
+#include "ros/subscribe_options.h"
+#include "std_msgs/Float32.h"
+
+namespace gazebo
+{
+ /// \brief A plugin to control the gripper.
+ class GripperPlugin : public ModelPlugin {
+
+ /// \brief Constructor
+ public: GripperPlugin() {}
+
+ public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
+ {
+ this->model = _model; // Store model pointer
+ this->sdf = _sdf; // Store sdf pointer
+
+ this->joint1 = this->model->GetJoint("cylinder_grip1");
+ this->joint2 = this->model->GetJoint("cylinder_grip2");
+ this->joint3 = this->model->GetJoint("cylinder_grip3");
+ this->joint4 = this->model->GetJoint("cylinder_grip4");
+ this->joint5 = this->model->GetJoint("cylinder_grip5");
+ this->joint6 = this->model->GetJoint("cylinder_grip6");
+
+ this->pid1 = common::PID(10, 1, 1.1);
+ this->pid2 = common::PID(10, 1, 1.1);
+ this->pid3 = common::PID(10, 1, 1.1);
+ this->pid4 = common::PID(10, 1, 1.1);
+ this->pid5 = common::PID(10, 1, 1.1);
+ this->pid6 = common::PID(10, 1, 1.1);
+
+ this->model->GetJointController()->SetPositionPID(this->joint1->GetScopedName(), this->pid1);
+ this->model->GetJointController()->SetPositionPID(this->joint2->GetScopedName(), this->pid2);
+ this->model->GetJointController()->SetPositionPID(this->joint3->GetScopedName(), this->pid3);
+ this->model->GetJointController()->SetPositionPID(this->joint4->GetScopedName(), this->pid4);
+ this->model->GetJointController()->SetPositionPID(this->joint5->GetScopedName(), this->pid5);
+ this->model->GetJointController()->SetPositionPID(this->joint6->GetScopedName(), this->pid6);
+
+ this->model->GetJointController()->SetPositionTarget(this->joint1->GetScopedName(), 0);
+ this->model->GetJointController()->SetPositionTarget(this->joint2->GetScopedName(), 0);
+ this->model->GetJointController()->SetPositionTarget(this->joint5->GetScopedName(), 0);
+
+ this->model->GetJointController()->SetPositionTarget(this->joint3->GetScopedName(), 0);
+ this->model->GetJointController()->SetPositionTarget(this->joint4->GetScopedName(), 0);
+ this->model->GetJointController()->SetPositionTarget(this->joint6->GetScopedName(), 0);
+
+// ¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤ ROS-related ¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤
+ if (!ros::isInitialized()) // Initialize ros, if it has not already been initialized.
+ {
+ int argc = 0;
+ char **argv = NULL;
+ ros::init(argc, argv, "gazebo_client",
+ ros::init_options::NoSigintHandler);
+ }
+
+ this->rosNode.reset(new ros::NodeHandle("gazebo_client")); // Create a ROS node
+
+ ros::SubscribeOptions so1 = // Create a ROS topic
+ ros::SubscribeOptions::create<std_msgs::Float32>(
+ "/" + this->model->GetName() + "/grip_rad", 1,
+ boost::bind(&GripperPlugin::OnRosMsg_grip, this, _1),
+ ros::VoidPtr(), &this->rosQueue1);
+
+ ros::SubscribeOptions so2 = // Create a ROS topic
+ ros::SubscribeOptions::create<std_msgs::Float32>(
+ "/" + this->model->GetName() + "/longgrip_rad", 1,
+ boost::bind(&GripperPlugin::OnRosMsg_longgrip, this, _1),
+ ros::VoidPtr(), &this->rosQueue2);
+
+ this->rosSub1 = this->rosNode->subscribe(so1); // Subscribe to the ROS topic.
+ this->rosSub2 = this->rosNode->subscribe(so2); // Subscribe to the ROS topic.
+
+ this->rosQueueThread1 = // Spin up the queue helper thread.
+ std::thread(std::bind(&GripperPlugin::QueueThread, this));
+ this->rosQueueThread2 = // Spin up the queue helper thread.
+ std::thread(std::bind(&GripperPlugin::QueueThread, this));
+// ¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤ /ROS-related ¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤
+ }
+
+ // -------GAZEBO-related-------
+ private: physics::JointPtr joint1; // Pointer to joint1.
+ private: physics::JointPtr joint2; // Pointer to joint2.
+ private: physics::JointPtr joint3; // Pointer to joint3.
+ private: physics::JointPtr joint4; // Pointer to joint3.
+ private: physics::JointPtr joint5; // Pointer to joint3.
+ private: physics::JointPtr joint6; // Pointer to joint3.
+ private: common::PID pid1; // A PID controller for joint1.
+ private: common::PID pid2; // A PID controller for joint2.
+ private: common::PID pid3; // A PID controller for joint3.
+ private: common::PID pid4; // A PID controller for joint3.
+ private: common::PID pid5; // A PID controller for joint3.
+ private: common::PID pid6; // A PID controller for joint3.
+
+ private: physics::ModelPtr model; // Pointer to the model.
+ private: sdf::ElementPtr sdf;
+
+ // -------ROS-related-------
+ private: std::unique_ptr<ros::NodeHandle> rosNode; // A node use for ROS transport
+ private: ros::Subscriber rosSub1; // A ROS subscriber
+ private: ros::Subscriber rosSub2; // A ROS subscriber
+ private: ros::CallbackQueue rosQueue1; // A ROS callbackqueue that helps process messages
+ private: ros::CallbackQueue rosQueue2; // A ROS callbackqueue that helps process messages
+ private: std::thread rosQueueThread1; // A thread the keeps running the rosQueue
+ private: std::thread rosQueueThread2; // A thread the keeps running the rosQueue
+
+ // ROS callback function
+ public: void OnRosMsg_longgrip(const std_msgs::Float32ConstPtr &_msg)
+ {
+ this->model->GetJointController()->SetPositionTarget(this->joint1->GetScopedName(), _msg->data);
+ this->model->GetJointController()->SetPositionTarget(this->joint2->GetScopedName(), _msg->data);
+ this->model->GetJointController()->SetPositionTarget(this->joint5->GetScopedName(), _msg->data);
+ }
+
+ public: void OnRosMsg_grip(const std_msgs::Float32ConstPtr &_msg)
+ {
+ this->model->GetJointController()->SetPositionTarget(this->joint3->GetScopedName(), _msg->data);
+ this->model->GetJointController()->SetPositionTarget(this->joint4->GetScopedName(), _msg->data);
+ this->model->GetJointController()->SetPositionTarget(this->joint6->GetScopedName(), _msg->data);
+ }
+
+ // ROS helper function that processes messages
+ private: void QueueThread()
+ {
+ static const double timeout = 0.01;
+ while (this->rosNode->ok())
+ {
+ this->rosQueue1.callAvailable(ros::WallDuration(timeout));
+ this->rosQueue2.callAvailable(ros::WallDuration(timeout));
+ }
+ }
+
+ };
+
+ // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
+ GZ_REGISTER_MODEL_PLUGIN(GripperPlugin)
+}
+#endif
diff --git a/simulation_control/CMakeLists.txt b/simulation_control/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..773c304635ada2918a51c55cfd526f19556b0c1e
--- /dev/null
+++ b/simulation_control/CMakeLists.txt
@@ -0,0 +1,211 @@
+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
+
+)
+
+## 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
+ short_grippers.action
+ long_grippers.action
+ )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+ DEPENDENCIES
+ actionlib_msgs
+ std_msgs
+ geometry_msgs
+ )
+
+################################################
+## 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)
diff --git a/simulation_control/README b/simulation_control/README
new file mode 100644
index 0000000000000000000000000000000000000000..12d2f4f1ad43d1d224796a136914e948d704f46c
--- /dev/null
+++ b/simulation_control/README
@@ -0,0 +1,14 @@
+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
+
+have fun
diff --git a/simulation_control/action/center_on_object.action b/simulation_control/action/center_on_object.action
new file mode 100644
index 0000000000000000000000000000000000000000..081b784919bd48ff080125c8d130d1a2342b511b
--- /dev/null
+++ b/simulation_control/action/center_on_object.action
@@ -0,0 +1,8 @@
+#goal definition
+std_msgs/String data
+---
+#result definition
+std_msgs/Bool centered
+---
+#feedback
+geometry_msgs/PoseStamped current_position
diff --git a/simulation_control/action/descend_on_object.action b/simulation_control/action/descend_on_object.action
new file mode 100644
index 0000000000000000000000000000000000000000..7b92238f83eb52ad378a5c063173f17f12663a0f
--- /dev/null
+++ b/simulation_control/action/descend_on_object.action
@@ -0,0 +1,8 @@
+#goal definition
+std_msgs/Float32 height
+---
+#result definition
+std_msgs/Bool position_reached
+---
+#feedback
+geometry_msgs/PoseStamped current_position
diff --git a/simulation_control/action/detect_object.action b/simulation_control/action/detect_object.action
new file mode 100644
index 0000000000000000000000000000000000000000..a164c32e61de1acd9a052836c83bf31553c22271
--- /dev/null
+++ b/simulation_control/action/detect_object.action
@@ -0,0 +1,8 @@
+#goal definition
+std_msgs/String data
+---
+#result definition
+geometry_msgs/PoseStamped detected_position
+---
+#feedback
+geometry_msgs/PoseStamped current_position
diff --git a/simulation_control/action/goto_position.action b/simulation_control/action/goto_position.action
new file mode 100644
index 0000000000000000000000000000000000000000..cacbd29e60cd4cce93443bf43443740370e452d3
--- /dev/null
+++ b/simulation_control/action/goto_position.action
@@ -0,0 +1,8 @@
+#goal definition
+geometry_msgs/PoseStamped destination
+---
+#result definition
+std_msgs/String data
+---
+#feedback
+geometry_msgs/PoseStamped current_position
diff --git a/simulation_control/action/long_grippers.action b/simulation_control/action/long_grippers.action
new file mode 100644
index 0000000000000000000000000000000000000000..c02b113bc56bfcc37e644fe99d6660859668d6b3
--- /dev/null
+++ b/simulation_control/action/long_grippers.action
@@ -0,0 +1,8 @@
+#goal definition
+std_msgs/Float32 grip_rad_goal
+---
+#result definition
+std_msgs/Bool goal_reached
+---
+#feedback
+std_msgs/Float32 grip_rad_current
diff --git a/simulation_control/action/short_grippers.action b/simulation_control/action/short_grippers.action
new file mode 100644
index 0000000000000000000000000000000000000000..c02b113bc56bfcc37e644fe99d6660859668d6b3
--- /dev/null
+++ b/simulation_control/action/short_grippers.action
@@ -0,0 +1,8 @@
+#goal definition
+std_msgs/Float32 grip_rad_goal
+---
+#result definition
+std_msgs/Bool goal_reached
+---
+#feedback
+std_msgs/Float32 grip_rad_current
diff --git a/simulation_control/package.xml b/simulation_control/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..2038730aa5dcb1bb7a4a57b5d3913029ef0b1571
--- /dev/null
+++ b/simulation_control/package.xml
@@ -0,0 +1,80 @@
+<?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>
diff --git a/simulation_control/src/Startup.txt b/simulation_control/src/Startup.txt
new file mode 100644
index 0000000000000000000000000000000000000000..3fb51d7076bd40e2291024cb0fda94959685b832
--- /dev/null
+++ b/simulation_control/src/Startup.txt
@@ -0,0 +1,14 @@
+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
+
+ready to fly
diff --git a/simulation_control/src/launch/posix_sitl.launch b/simulation_control/src/launch/posix_sitl.launch
new file mode 100644
index 0000000000000000000000000000000000000000..95e8f082457f75d2dfc8846ade584e358edac557
--- /dev/null
+++ b/simulation_control/src/launch/posix_sitl.launch
@@ -0,0 +1,44 @@
+<launch>
+
+ <!-- Posix SITL environment launch script -->
+ <arg name="x" default="0"/>
+ <arg name="y" default="0"/>
+ <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="f550_amazing"/>
+ <arg name="world" default="$(find simulation_control)/src/worlds/timpa_airfield.world"/>
+ <arg name="sdf" default="$(find simulation_control)/src/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="true"/>
+ <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 : -->
diff --git a/simulation_control/src/launch/px4.launch b/simulation_control/src/launch/px4.launch
new file mode 100644
index 0000000000000000000000000000000000000000..2718085583adf425973e9ee293d2c7f081e074d6
--- /dev/null
+++ b/simulation_control/src/launch/px4.launch
@@ -0,0 +1,23 @@
+<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>
diff --git a/simulation_control/src/launch/simulation_control.launch b/simulation_control/src/launch/simulation_control.launch
new file mode 100644
index 0000000000000000000000000000000000000000..c2a5d01bcb2ac9dea54d6b96484e4b9db44de43f
--- /dev/null
+++ b/simulation_control/src/launch/simulation_control.launch
@@ -0,0 +1,10 @@
+<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="short_grippers_server" pkg="simulation_control" type="short_grippers_server.py" output="screen" />
+ <node name="long_grippers_server" pkg="simulation_control" type="long_grippers_server.py" output="screen" />
+ <node name="descend_on_object_server" pkg="simulation_control" type="descend_on_object_server.py" output="screen" />
+</launch>
diff --git a/simulation_control/src/models/f550_amazing/f550_amazing.sdf b/simulation_control/src/models/f550_amazing/f550_amazing.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..564d2baea9e42cec0a2ed524a32740e654c51515
--- /dev/null
+++ b/simulation_control/src/models/f550_amazing/f550_amazing.sdf
@@ -0,0 +1,1675 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <model name="f550_amazing">
+ <static>false</static>
+ <pose>0 0 0.35 0 0 0</pose>
+
+<!-- ######################################## LOWER_PLATE # ######################################## -->
+ <link name="base_link">
+ <gravity>true</gravity>
+ <pose>0 0 0.0015 0 0 0</pose>
+
+ <inertial>
+ <pose frame=''>0.001005 0 -0.0090035 0 -0 0</pose>
+ <mass>0.6</mass>
+ <inertia>
+ <ixx>0.011</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.015</iyy>
+ <iyz>0</iyz>
+ <izz>0.021</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="base_link_visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Lower_plate_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+<!-- ####################################### /LOWER_PLATE # ######################################## -->
+
+
+<!--
+ <joint name="fixed to ground" type="fixed">
+ <parent>world</parent>
+ <child>base_link</child>
+ </joint>
+-->
+
+
+<!-- ######################################## TOP_PLATE ############################################ -->
+ <link name="Top_plate_link">
+ <gravity>true</gravity>
+ <pose>0 0 0.0375 0 0 2.09439510239</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Top_plate">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Top_plate_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Top_joint' type='fixed'>
+ <child>Top_plate_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ####################################### /TOP_PLATE ############################################ -->
+
+<!-- ########################################## CAMERA ########################################### -->
+ <link name='camera_down_link'>
+ <pose>0 0 0 0 1.57079 3.14159265359</pose>
+
+ <inertial>
+ <mass>0.001</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+ <gravity>1</gravity>
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+
+ <visual name='visual_camera_down'>
+ <pose>0 0 0 0 -1.57079 0</pose>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/oCam-1MGN-U_CG.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/Grey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+
+ <sensor name='camera' type='camera'>
+ <always_on>1</always_on>
+ <update_rate>30</update_rate>
+ <camera name='__default__'>
+ <horizontal_fov>1.13446</horizontal_fov>
+ <image>
+ <width>1280</width>
+ <height>720</height>
+ </image>
+ <clip>
+ <near>0.1</near>
+ <far>100</far>
+ </clip>
+ </camera>
+ <plugin name='camera_plugin' filename='libgazebo_ros_camera.so'>
+ <alwaysOn>true</alwaysOn>
+ <imageTopicName>image_raw_down</imageTopicName>
+ <cameraInfoTopicName>camera_info</cameraInfoTopicName>
+ <updateRate>10.0</updateRate>
+ <cameraName>uav_camera</cameraName>
+ <frameName>f550_amazing/robot_camera_down</frameName>
+ <CxPrime>640.5</CxPrime>
+ <Cx>640.5</Cx>
+ <Cy>360.5</Cy>
+ <hackBaseline>0</hackBaseline>
+ <focalLength>1004.603218</focalLength>
+ <distortionK1>0.0</distortionK1>
+ <distortionK2>0.0</distortionK2>
+ <distortionK3>0.0</distortionK3>
+ <distortionT1>0.0</distortionT1>
+ <distortionT2>0.0</distortionT2>
+ <robotNamespace>/</robotNamespace>
+ </plugin>
+ </sensor>
+
+ </link>
+
+ <joint name='camera_down_joint' type='fixed'>
+ <parent>base_link</parent>
+ <child>camera_down_link</child>
+ </joint>
+
+<!-- ########################################## /CAMERA ########################################## -->
+
+<!-- ########################################## IMU ############################################## -->
+ <link name='imu_link'>
+ <pose frame=''>0 0 0 0 0 3.1415927</pose>
+ <inertial>
+ <pose frame=''>0 0 0 0 -0 0</pose>
+ <mass>0.015</mass>
+ <inertia>
+ <ixx>1e-05</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>1e-05</iyy>
+ <iyz>0</iyz>
+ <izz>1e-05</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_imu">
+ <pose frame=''>0 0 0 0 0 3.1415927</pose>
+ <geometry>
+ <box>
+ <size>0.01 0.02 0.02</size>
+ </box>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='imu_joint' type='revolute'>
+ <child>imu_link</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0</lower>
+ <upper>0</upper>
+ <effort>0</effort>
+ <velocity>0</velocity>
+ </limit>
+ <dynamics>
+ <spring_reference>0</spring_reference>
+ <spring_stiffness>0</spring_stiffness>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ </joint>
+<!-- ######################################### /IMU ############################################## -->
+
+<!-- ############################################# ARM1 ############################################ -->
+ <link name="Arm1_link">
+ <gravity>true</gravity>
+ <pose>0 0.1874 0.009875004 0 0 1.57079632679</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+<!--
+ <collision name="collision_Arm1_box">
+ <pose>-0.02 0 0.009 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.175 0.034 0.037</size>
+ </box>
+ </geometry>
+ </collision>
+
+ <collision name="collision_Arm1_cyl">
+ <pose>0.09 0 0.02 0 0 0</pose>
+ <geometry>
+ <cylinder>
+ <length>0.097</length>
+ <radius>0.02</radius>
+ </cylinder>
+ </geometry>
+ </collision>
+-->
+
+ <visual name="visual_Arm1">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm1_joint' type='fixed'>
+ <child>Arm1_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM1 ############################################ -->
+
+<!-- ############################################# ARM2 ############################################ -->
+ <link name="Arm2_link">
+ <gravity>true</gravity>
+ <pose>0.162293160669 0.0937 0.009875004 0 0 0.523598775598</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm2">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm2_joint' type='fixed'>
+ <child>Arm2_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM2 ############################################ -->
+
+<!-- ############################################# ARM3 ############################################ -->
+ <link name="Arm3_link">
+ <gravity>true</gravity>
+ <pose>0.162293160669 -0.0937 0.009875004 0 0 -0.523598775598</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm3">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm3_joint' type='fixed'>
+ <child>Arm3_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM3 ############################################ -->
+
+<!-- ############################################# ARM4 ############################################ -->
+ <link name="Arm4_link">
+ <gravity>true</gravity>
+ <pose>0 -0.1874 0.009875004 0 0 -1.57079632679</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm4">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm4_joint' type='fixed'>
+ <child>Arm4_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM4 ############################################ -->
+
+<!-- ############################################# ARM5 ############################################ -->
+ <link name="Arm5_link">
+ <gravity>true</gravity>
+ <pose>-0.162293160669 -0.0937 0.009875004 0 0 3.66519142919</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm5">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm5_joint' type='fixed'>
+ <child>Arm5_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM5 ############################################ -->
+
+<!-- ############################################# ARM6 ############################################ -->
+ <link name="Arm6_link">
+ <gravity>true</gravity>
+ <pose>-0.162293160669 0.0937 0.009875004 0 0 2.61799387799</pose>
+
+ <inertial>
+ <mass>0.1</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm6">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm6_joint' type='fixed'>
+ <child>Arm6_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM6 ############################################ -->
+
+<!-- ######################################### ROTOR0 ############################################ -->
+<link name='rotor_0'>
+ <pose frame=''>-0.279726205 0.1615 0.07 0 0 2.09439510239</pose>
+ <inertial>
+ <mass>0.005</mass>
+ <inertia>
+ <ixx>9.75e-07</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.000273104</iyy>
+ <iyz>0</iyz>
+ <izz>0.000274004</izz>
+ </inertia>
+ </inertial>
+
+<!--
+ <collision name='rotor_0_collision'>
+ <geometry>
+ <cylinder>
+ <length>0.01</length>
+ <radius>0.128</radius>
+ </cylinder>
+ </geometry>
+ <surface>
+ <contact>
+ <ode/>
+ </contact>
+ <friction>
+ <ode/>
+ </friction>
+ </surface>
+ </collision>
+-->
+
+ <visual name='rotor_0_visual'>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/prop_cw_CG.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='rotor_0_joint' type='revolute'>
+ <child>rotor_0</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0.046 0.0827 1.8977</xyz>
+ <limit>
+ <lower>-1e+16</lower>
+ <upper>1e+16</upper>
+ <effort>10</effort>
+ <velocity>-1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.005</damping>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+<!-- ######################################## /ROTOR0 ############################################ -->
+
+<!-- ######################################### ROTOR1 ############################################ -->
+<link name='rotor_1'>
+ <pose frame=''>0.279726205 -0.1615 0.07 0 0 -2.09439510239</pose>
+ <inertial>
+ <mass>0.005</mass>
+ <inertia>
+ <ixx>9.75e-07</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.000273104</iyy>
+ <iyz>0</iyz>
+ <izz>0.000274004</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='rotor_1_visual'>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/prop_ccw_CG.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='rotor_1_joint' type='revolute'>
+ <child>rotor_1</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0.046 0.0827 1.8977</xyz>
+ <limit>
+ <lower>-1e+16</lower>
+ <upper>1e+16</upper>
+ <effort>10</effort>
+ <velocity>-1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.005</damping>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+<!-- ######################################## /ROTOR1 ############################################ -->
+
+<!-- ######################################### ROTOR2 ############################################ -->
+<link name='rotor_2'>
+ <pose frame=''>-0.279726205 -0.1615 0.07 0 0 2.09439510239</pose>
+ <inertial>
+ <mass>0.005</mass>
+ <inertia>
+ <ixx>9.75e-07</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.000273104</iyy>
+ <iyz>0</iyz>
+ <izz>0.000274004</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='rotor_2_visual'>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/prop_ccw_CG.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='rotor_2_joint' type='revolute'>
+ <child>rotor_2</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0.046 0.0827 1.8977</xyz>
+ <limit>
+ <lower>-1e+16</lower>
+ <upper>1e+16</upper>
+ <effort>10</effort>
+ <velocity>-1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.005</damping>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+<!-- ######################################## /ROTOR2 ############################################ -->
+
+<!-- ######################################### ROTOR3 ############################################ -->
+<link name='rotor_3'>
+ <pose frame=''>0.279726205 0.1615 0.07 0 0 0</pose>
+ <inertial>
+ <mass>0.005</mass>
+ <inertia>
+ <ixx>9.75e-07</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.000273104</iyy>
+ <iyz>0</iyz>
+ <izz>0.000274004</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='rotor_3_visual'>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/prop_cw_CG.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='rotor_3_joint' type='revolute'>
+ <child>rotor_3</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0.046 0.0827 1.8977</xyz>
+ <limit>
+ <lower>-1e+16</lower>
+ <upper>1e+16</upper>
+ <effort>10</effort>
+ <velocity>-1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.005</damping>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+<!-- ######################################## /ROTOR3 ############################################ -->
+
+<!-- ######################################### ROTOR4 ############################################ -->
+<link name='rotor_4'>
+ <pose frame=''>0 0.323 0.07 0 0 0</pose>
+ <inertial>
+ <mass>0.005</mass>
+ <inertia>
+ <ixx>9.75e-07</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.000273104</iyy>
+ <iyz>0</iyz>
+ <izz>0.000274004</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='rotor_4_visual'>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/prop_ccw_CG.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='rotor_4_joint' type='revolute'>
+ <child>rotor_4</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0.046 0.0827 1.8977</xyz>
+ <limit>
+ <lower>-1e+16</lower>
+ <upper>1e+16</upper>
+ <effort>10</effort>
+ <velocity>-1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.005</damping>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+<!-- ######################################## /ROTOR4 ############################################ -->
+
+<!-- ######################################### ROTOR5 ############################################ -->
+<link name='rotor_5'>
+ <pose frame=''>0 -0.323 0.07 0 0 -2.09439510239</pose>
+ <inertial>
+ <mass>0.005</mass>
+ <inertia>
+ <ixx>9.75e-07</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.000273104</iyy>
+ <iyz>0</iyz>
+ <izz>0.000274004</izz>
+ </inertia>
+ </inertial>
+
+ <visual name='rotor_5_visual'>
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/prop_ccw_CG.stl</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='rotor_5_joint' type='revolute'>
+ <child>rotor_5</child>
+ <parent>base_link</parent>
+ <axis>
+ <xyz>0.046 0.0827 1.8977</xyz>
+ <limit>
+ <lower>-1e+16</lower>
+ <upper>1e+16</upper>
+ <effort>10</effort>
+ <velocity>-1</velocity>
+ </limit>
+ <dynamics>
+ <damping>0.005</damping>
+ </dynamics>
+ <use_parent_model_frame>1</use_parent_model_frame>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>
+ </ode>
+ </physics>
+ </joint>
+<!-- ######################################## /ROTOR5 ############################################ -->
+
+<!-- ######################################### grip1 ############################################### -->
+
+ <link name="cylinder1">
+ <pose>0 -0.0967 0 0 0 0</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/cylinder.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="base_link_cylinder1" type="fixed">
+ <child>cylinder1</child>
+ <parent>base_link</parent>
+ </joint>
+
+ <link name="grip1">
+ <pose>0 -0.0967 -0.02 0 0 0</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip_long.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip_long.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="cylinder_grip1" type="revolute">
+ <pose>0 0 0 0 0 0</pose>
+ <child>grip1</child>
+ <parent>cylinder1</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0.001</lower>
+ <upper>1.570</upper>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>5</damping>
+ <friction>0.5</friction>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>'
+ </ode>
+ </physics>
+ </joint>
+
+<!-- ######################################### /grip1 ############################################### -->
+
+<!-- ######################################### grip2 ############################################### -->
+
+ <link name="cylinder2">
+ <pose>0.0837446656546 0.04835 0 0 0 2.09439510239</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/cylinder.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="base_link_cylinder2" type="fixed">
+ <child>cylinder2</child>
+ <parent>base_link</parent>
+ </joint>
+
+ <link name="grip2">
+ <pose>0.0837446656546 0.04835 -0.02 0 0 2.09439510239</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip_long.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip_long.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="cylinder_grip2" type="revolute">
+ <pose>0 0 0 0 0 0</pose>
+ <child>grip2</child>
+ <parent>cylinder2</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0.001</lower>
+ <upper>1.570</upper>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>5</damping>
+ <friction>0.5</friction>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>'
+ </ode>
+ </physics>
+ </joint>
+
+<!-- ######################################### /grip2 ############################################### -->
+
+<!-- ######################################### grip3 ############################################### -->
+
+ <link name="cylinder3">
+ <pose>0.0837446656546 -0.04835 0 0 0 1.0471975512</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/cylinder.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="base_link_cylinder3" type="fixed">
+ <child>cylinder3</child>
+ <parent>base_link</parent>
+ </joint>
+
+ <link name="grip3">
+ <pose>0.0837446656546 -0.04835 -0.02 0 0 1.0471975512</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="cylinder_grip3" type="revolute">
+ <pose>0 0 0 0 0 0</pose>
+ <child>grip3</child>
+ <parent>cylinder3</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0.001</lower>
+ <upper>1.570</upper>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>5</damping>
+ <friction>0.5</friction>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>'
+ </ode>
+ </physics>
+ </joint>
+
+<!-- ######################################### /grip3 ############################################### -->
+
+<!-- ######################################### grip4 ############################################### -->
+
+ <link name="cylinder4">
+ <pose>0 0.0967 0 0 0 0</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/cylinder.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="base_link_cylinder4" type="fixed">
+ <child>cylinder4</child>
+ <parent>base_link</parent>
+ </joint>
+
+ <link name="grip4">
+ <pose>0 0.0967 -0.02 0 0 3.14159265359</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="cylinder_grip4" type="revolute">
+ <pose>0 0 0 0 0 0</pose>
+ <child>grip4</child>
+ <parent>cylinder4</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0.001</lower>
+ <upper>1.570</upper>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>5</damping>
+ <friction>0.5</friction>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>'
+ </ode>
+ </physics>
+ </joint>
+
+<!-- ######################################### /grip4 ############################################### -->
+
+<!-- ######################################### grip5 ############################################### -->
+
+ <link name="cylinder5">
+ <pose>-0.0837446656546 0.04835 0 0 0 4.18879020479</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/cylinder.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="base_link_cylinder5" type="fixed">
+ <child>cylinder5</child>
+ <parent>base_link</parent>
+ </joint>
+
+ <link name="grip5">
+ <pose>-0.0837446656546 0.04835 -0.02 0 0 4.18879020479</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip_long.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip_long.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="cylinder_grip5" type="revolute">
+ <pose>0 0 0 0 0 0</pose>
+ <child>grip5</child>
+ <parent>cylinder5</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0.001</lower>
+ <upper>1.570</upper>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>5</damping>
+ <friction>0.5</friction>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>'
+ </ode>
+ </physics>
+ </joint>
+
+<!-- ######################################### /grip5 ############################################### -->
+
+<!-- ######################################### grip6 ############################################### -->
+
+ <link name="cylinder6">
+ <pose>-0.0837446656546 -0.04835 0 0 0 -1.0471975512</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/cylinder.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="base_link_cylinder6" type="fixed">
+ <child>cylinder6</child>
+ <parent>base_link</parent>
+ </joint>
+
+ <link name="grip6">
+ <pose>-0.0837446656546 -0.04835 -0.02 0 0 -1.0471975512</pose>
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip.stl</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ </surface>
+ </collision>
+
+ <visual name="visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/grip.stl</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name="cylinder_grip6" type="revolute">
+ <pose>0 0 0 0 0 0</pose>
+ <child>grip6</child>
+ <parent>cylinder6</parent>
+ <axis>
+ <xyz>1 0 0</xyz>
+ <limit>
+ <lower>0.001</lower>
+ <upper>1.570</upper>
+ <velocity>1</velocity>
+ </limit>
+ <dynamics>
+ <damping>5</damping>
+ <friction>0.5</friction>
+ </dynamics>
+ </axis>
+ <physics>
+ <ode>
+ <implicit_spring_damper>1</implicit_spring_damper>'
+ </ode>
+ </physics>
+ </joint>
+
+<!-- ######################################### /grip6 ############################################### -->
+
+<!-- ###################################### GRIPPER PLUGIN ########################################## -->
+ <plugin name="hex_gripper_control" filename="libgripper_plugin.so" />
+<!-- ###################################### /GRIPPER PLUGIN ######################################### -->
+
+<!-- ##################################### MOTOR PLUGINS ######################################### -->
+ <plugin name='front_right_motor_model' filename='librotors_gazebo_motor_model.so'>
+ <robotNamespace></robotNamespace>
+ <jointName>rotor_0_joint</jointName>
+ <linkName>rotor_0</linkName>
+ <turningDirection>ccw</turningDirection>
+ <timeConstantUp>0.0125</timeConstantUp>
+ <timeConstantDown>0.025</timeConstantDown>
+ <maxRotVelocity>1100</maxRotVelocity>
+ <motorConstant>8.54858e-06</motorConstant>
+ <momentConstant>0.06</momentConstant>
+ <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+ <motorNumber>4</motorNumber>
+ <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
+ <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+ <motorSpeedPubTopic>/motor_speed/4</motorSpeedPubTopic>
+ <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+ </plugin>
+
+ <plugin name='back_left_motor_model' filename='librotors_gazebo_motor_model.so'>
+ <robotNamespace></robotNamespace>
+ <jointName>rotor_1_joint</jointName>
+ <linkName>rotor_1</linkName>
+ <turningDirection>cw</turningDirection>
+ <timeConstantUp>0.0125</timeConstantUp>
+ <timeConstantDown>0.025</timeConstantDown>
+ <maxRotVelocity>1100</maxRotVelocity>
+ <motorConstant>8.54858e-06</motorConstant>
+ <momentConstant>0.06</momentConstant>
+ <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+ <motorNumber>5</motorNumber>
+ <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
+ <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+ <motorSpeedPubTopic>/motor_speed/5</motorSpeedPubTopic>
+ <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+ </plugin>
+
+ <plugin name='front_left_motor_model' filename='librotors_gazebo_motor_model.so'>
+ <robotNamespace></robotNamespace>
+ <jointName>rotor_2_joint</jointName>
+ <linkName>rotor_2</linkName>
+ <turningDirection>cw</turningDirection>
+ <timeConstantUp>0.0125</timeConstantUp>
+ <timeConstantDown>0.025</timeConstantDown>
+ <maxRotVelocity>1100</maxRotVelocity>
+ <motorConstant>8.54858e-06</motorConstant>
+ <momentConstant>0.06</momentConstant>
+ <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+ <motorNumber>2</motorNumber>
+ <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
+ <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+ <motorSpeedPubTopic>/motor_speed/2</motorSpeedPubTopic>
+ <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+ </plugin>
+
+ <plugin name='back_right_motor_model' filename='librotors_gazebo_motor_model.so'>
+ <robotNamespace></robotNamespace>
+ <jointName>rotor_3_joint</jointName>
+ <linkName>rotor_3</linkName>
+ <turningDirection>ccw</turningDirection>
+ <timeConstantUp>0.0125</timeConstantUp>
+ <timeConstantDown>0.025</timeConstantDown>
+ <maxRotVelocity>1100</maxRotVelocity>
+ <motorConstant>8.54858e-06</motorConstant>
+ <momentConstant>0.06</momentConstant>
+ <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+ <motorNumber>3</motorNumber>
+ <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
+ <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+ <motorSpeedPubTopic>/motor_speed/3</motorSpeedPubTopic>
+ <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+ </plugin>
+
+ <plugin name='back_left_motor_model' filename='librotors_gazebo_motor_model.so'>
+ <robotNamespace></robotNamespace>
+ <jointName>rotor_4_joint</jointName>
+ <linkName>rotor_4</linkName>
+ <turningDirection>cw</turningDirection>
+ <timeConstantUp>0.0125</timeConstantUp>
+ <timeConstantDown>0.025</timeConstantDown>
+ <maxRotVelocity>1100</maxRotVelocity>
+ <motorConstant>8.54858e-06</motorConstant>
+ <momentConstant>0.06</momentConstant>
+ <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+ <motorNumber>0</motorNumber>
+ <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
+ <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+ <motorSpeedPubTopic>/motor_speed/0</motorSpeedPubTopic>
+ <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+ </plugin>
+
+ <plugin name='front_left_motor_model' filename='librotors_gazebo_motor_model.so'>
+ <robotNamespace></robotNamespace>
+ <jointName>rotor_5_joint</jointName>
+ <linkName>rotor_5</linkName>
+ <turningDirection>ccw</turningDirection>
+ <timeConstantUp>0.0125</timeConstantUp>
+ <timeConstantDown>0.025</timeConstantDown>
+ <maxRotVelocity>1100</maxRotVelocity>
+ <motorConstant>8.54858e-06</motorConstant>
+ <momentConstant>0.06</momentConstant>
+ <commandSubTopic>/gazebo/command/motor_speed</commandSubTopic>
+ <motorNumber>1</motorNumber>
+ <rotorDragCoefficient>0.000806428</rotorDragCoefficient>
+ <rollingMomentCoefficient>1e-06</rollingMomentCoefficient>
+ <motorSpeedPubTopic>/motor_speed/1</motorSpeedPubTopic>
+ <rotorVelocitySlowdownSim>10</rotorVelocitySlowdownSim>
+ </plugin>
+<!-- #################################### /MOTOR PLUGINS ######################################### -->
+
+<!-- ################################### MAVLINK PLUGIN ########################################## -->
+ <plugin name='mavlink_interface' filename='librotors_gazebo_mavlink_interface.so'>
+ <robotNamespace></robotNamespace>
+ <imuSubTopic>/imu</imuSubTopic>
+ <motorSpeedCommandPubTopic>/gazebo/command/motor_speed</motorSpeedCommandPubTopic>
+ <control_channels>
+
+ <channel name="rotor0">
+ <input_index>0</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>450</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>100</zero_position_armed>
+ <joint_control_type>velocity</joint_control_type>
+ <joint_name>rotor_4_joint</joint_name>
+ </channel>
+
+ <channel name="rotor1">
+ <input_index>1</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>450</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>100</zero_position_armed>
+ <joint_control_type>velocity</joint_control_type>
+
+ <joint_name>rotor_5_joint</joint_name>
+ </channel>
+
+ <channel name="rotor2">
+ <input_index>2</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>450</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>100</zero_position_armed>
+ <joint_control_type>velocity</joint_control_type>
+ <joint_name>rotor_2_joint</joint_name>
+ </channel>
+
+ <channel name="rotor3">
+ <input_index>3</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>450</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>100</zero_position_armed>
+ <joint_control_type>velocity</joint_control_type>
+ <joint_name>rotor_3_joint</joint_name>
+ </channel>
+
+ <channel name="rotor4">
+ <input_index>4</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>450</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>100</zero_position_armed>
+ <joint_control_type>velocity</joint_control_type>
+ <joint_name>rotor_0_joint</joint_name>
+ </channel>
+
+ <channel name="rotor5">
+ <input_index>5</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>450</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>100</zero_position_armed>
+ <joint_control_type>velocity</joint_control_type>
+ <joint_name>rotor_1_joint</joint_name>
+ </channel>
+
+ <channel name="gimbal_roll">
+ <input_index>6</input_index>
+ <input_offset>0</input_offset>
+ <input_scaling>-3.1415</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>0</zero_position_armed>
+ <joint_control_type>position_gztopic</joint_control_type>
+ <gztopic>/gimbal_roll_cmd</gztopic>
+ <joint_name>typhoon_h480::cgo3_camera_joint</joint_name>
+ </channel>
+
+ <channel name="gimbal_pitch">
+ <input_index>7</input_index>
+ <input_offset>0</input_offset>
+ <input_scaling>3.1415</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>0</zero_position_armed>
+ <joint_control_type>position_gztopic</joint_control_type>
+ <gztopic>/gimbal_pitch_cmd</gztopic>
+ <joint_name>typhoon_h480::cgo3_camera_joint</joint_name>
+ </channel>
+
+ <channel name="gimbal_yaw">
+ <input_index>8</input_index>
+ <input_offset>-0.5</input_offset>
+ <input_scaling>-3.1415</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>0</zero_position_armed>
+ <joint_control_type>position_gztopic</joint_control_type>
+ <gztopic>/gimbal_yaw_cmd</gztopic>
+ <joint_name>typhoon_h480::cgo3_vertical_arm_joint</joint_name>
+ </channel>
+
+ <channel name="left_leg">
+ <input_index>9</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>1</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>0</zero_position_armed>
+ <joint_control_type>position</joint_control_type>
+ <joint_control_pid>
+ <p>50.0</p>
+ <i>0</i>
+ <d>0</d>
+ <iMax>0</iMax>
+ <iMin>0</iMin>
+ <cmdMax>10</cmdMax>
+ <cmdMin>-10</cmdMin>
+ </joint_control_pid>
+ <joint_name>left_leg_joint</joint_name>
+ </channel>
+
+ <channel name="right_leg">
+ <input_index>10</input_index>
+ <input_offset>1</input_offset>
+ <input_scaling>1</input_scaling>
+ <zero_position_disarmed>0</zero_position_disarmed>
+ <zero_position_armed>0</zero_position_armed>
+ <joint_control_type>position</joint_control_type>
+ <joint_control_pid>
+ <p>50.0</p>
+ <i>0</i>
+ <d>0</d>
+ <iMax>0</iMax>
+ <iMin>0</iMin>
+ <cmdMax>10</cmdMax>
+ <cmdMin>-10</cmdMin>
+ </joint_control_pid>
+ <joint_name>right_leg_joint</joint_name>
+ </channel>
+ </control_channels>
+ </plugin>
+
+<!-- ################################### /MAVLINK PLUGIN ######################################### -->
+
+<!-- ################################### IMU PLUGIN ############################################## -->
+ <plugin name='rotors_gazebo_imu_plugin' filename='librotors_gazebo_imu_plugin.so'>
+ <robotNamespace></robotNamespace>
+ <linkName>imu_link</linkName>
+ <imuTopic>/imu</imuTopic>
+ <gyroscopeNoiseDensity>0</gyroscopeNoiseDensity>
+ <gyroscopeRandomWalk>0</gyroscopeRandomWalk>
+ <gyroscopeBiasCorrelationTime>1000.0</gyroscopeBiasCorrelationTime>
+ <gyroscopeTurnOnBiasSigma>0</gyroscopeTurnOnBiasSigma>
+ <accelerometerNoiseDensity>0</accelerometerNoiseDensity>
+ <accelerometerRandomWalk>0</accelerometerRandomWalk>
+ <accelerometerBiasCorrelationTime>300.0</accelerometerBiasCorrelationTime>
+ <accelerometerTurnOnBiasSigma>0</accelerometerTurnOnBiasSigma>
+ </plugin>
+<!-- ################################## /IMU PLUGIN ############################################## -->
+
+ </model>
+</sdf>
diff --git a/simulation_control/src/models/f550_amazing/meshes/Arm_CG.STL b/simulation_control/src/models/f550_amazing/meshes/Arm_CG.STL
new file mode 100644
index 0000000000000000000000000000000000000000..dd3c49b7e7da77375a966f4ddab1eef57819395a
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/Arm_CG.STL differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/Lower_plate_CG.STL b/simulation_control/src/models/f550_amazing/meshes/Lower_plate_CG.STL
new file mode 100644
index 0000000000000000000000000000000000000000..12c6fc8d62855b56f9bbd91cd1706d2148694475
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/Lower_plate_CG.STL differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/Top_plate_CG.STL b/simulation_control/src/models/f550_amazing/meshes/Top_plate_CG.STL
new file mode 100644
index 0000000000000000000000000000000000000000..f4aa28638db123d5e1f212755bcc685f247cd716
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/Top_plate_CG.STL differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/cylinder.stl b/simulation_control/src/models/f550_amazing/meshes/cylinder.stl
new file mode 100644
index 0000000000000000000000000000000000000000..cfd58e386c52343f394611037b87675c3a20aa29
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/cylinder.stl differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/grip.stl b/simulation_control/src/models/f550_amazing/meshes/grip.stl
new file mode 100644
index 0000000000000000000000000000000000000000..dc481beb33fdd93f0ea5f6913ee9407ed8f75077
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/grip.stl differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/grip_long.stl b/simulation_control/src/models/f550_amazing/meshes/grip_long.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e1b2b2dd6b6c2cd4f8da289724084ae3f07bfe30
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/grip_long.stl differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/grip_non_vert.stl b/simulation_control/src/models/f550_amazing/meshes/grip_non_vert.stl
new file mode 100644
index 0000000000000000000000000000000000000000..a99db53a60093fcd0fd631dcedf7febf89eac099
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/grip_non_vert.stl differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/oCam-1MGN-U_CG.stl b/simulation_control/src/models/f550_amazing/meshes/oCam-1MGN-U_CG.stl
new file mode 100644
index 0000000000000000000000000000000000000000..41ba3f4a60a172f9c9f6469269f962cb1435fcc2
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/oCam-1MGN-U_CG.stl differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/prop_ccw_CG.stl b/simulation_control/src/models/f550_amazing/meshes/prop_ccw_CG.stl
new file mode 100644
index 0000000000000000000000000000000000000000..1ab02effb997c977bcfe155395aeda1fc07e5323
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/prop_ccw_CG.stl differ
diff --git a/simulation_control/src/models/f550_amazing/meshes/prop_cw_CG.stl b/simulation_control/src/models/f550_amazing/meshes/prop_cw_CG.stl
new file mode 100644
index 0000000000000000000000000000000000000000..882de41086b99d797cb224cb552d85aabef1c4b1
Binary files /dev/null and b/simulation_control/src/models/f550_amazing/meshes/prop_cw_CG.stl differ
diff --git a/simulation_control/src/models/f550_amazing/model.config b/simulation_control/src/models/f550_amazing/model.config
new file mode 100644
index 0000000000000000000000000000000000000000..7e1b9047c38a68c9b3eefe3e795962398a8492c3
--- /dev/null
+++ b/simulation_control/src/models/f550_amazing/model.config
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+ <name>f550_amazing</name>
+ <version>1.0</version>
+ <sdf version="1.5">f550_amazing.sdf</sdf>
+
+ <author>
+ <name>Anders Bogga</name>
+ <email>andorsk@gmail.com</email>
+ </author>
+
+ <description>
+ f550_amazing
+ </description>
+</model>
diff --git a/simulation_control/src/models/f550_amazing_dead_blue/f550_amazing_dead_blue.sdf b/simulation_control/src/models/f550_amazing_dead_blue/f550_amazing_dead_blue.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..8e70306e470aa90fc397d18a0b8b39007fe91706
--- /dev/null
+++ b/simulation_control/src/models/f550_amazing_dead_blue/f550_amazing_dead_blue.sdf
@@ -0,0 +1,371 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+ <model name="f550_amazing_dead_blue">
+ <static>false</static>
+ <pose>0 0 0 0 0 0</pose>
+
+<!-- ######################################## LOWER_PLATE # ######################################## -->
+ <link name="base_link">
+ <gravity>true</gravity>
+ <pose>0 0 0.0015 0 0 0</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="base_link_collision">
+ <pose>0 0 -0.01 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>0.179 0.179 0.003</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>1</mu>
+ <mu2>1</mu2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+
+ <visual name="base_link_visual">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Lower_plate_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+<!-- ####################################### /LOWER_PLATE # ######################################## -->
+
+<!-- ######################################## TOP_PLATE ############################################ -->
+ <link name="Top_plate_link">
+ <gravity>true</gravity>
+ <pose>0 0 0.0375 0 0 2.09439510239</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.01</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.01</iyy>
+ <iyz>0</iyz>
+ <izz>0.01</izz>
+ </inertia>
+ </inertial>
+
+ <collision name="collision_Top_plate">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Top_plate_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>1</mu>
+ <mu2>1</mu2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ </collision>
+
+ <visual name="visual_Top_plate">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Top_plate_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+
+
+ <visual name="visual_Top_plate_Blue">
+ <pose>0 0 0 0 0 0.523599</pose>
+ <geometry>
+ <box>
+ <size>0.179 0.179 0.006</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>
+ </visual>
+
+ </link>
+
+ <joint name='Lower_Top_joint' type='fixed'>
+ <child>Top_plate_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ####################################### /TOP_PLATE ############################################ -->
+
+<!-- ############################################# ARM1 ############################################ -->
+ <link name="Arm1_link">
+ <gravity>true</gravity>
+ <pose>0 0.1874 0.009875004 0 0 1.57079632679</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm1">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm1_joint' type='fixed'>
+ <child>Arm1_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM1 ############################################ -->
+
+<!-- ############################################# ARM2 ############################################ -->
+ <link name="Arm2_link">
+ <gravity>true</gravity>
+ <pose>0.162293160669 0.0937 0.009875004 0 0 0.523598775598</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm2">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm2_joint' type='fixed'>
+ <child>Arm2_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM2 ############################################ -->
+
+<!-- ############################################# ARM3 ############################################ -->
+ <link name="Arm3_link">
+ <gravity>true</gravity>
+ <pose>0.162293160669 -0.0937 0.009875004 0 0 -0.523598775598</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm3">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm3_joint' type='fixed'>
+ <child>Arm3_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM3 ############################################ -->
+
+<!-- ############################################# ARM4 ############################################ -->
+ <link name="Arm4_link">
+ <gravity>true</gravity>
+ <pose>0 -0.1874 0.009875004 0 0 -1.57079632679</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm4">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm4_joint' type='fixed'>
+ <child>Arm4_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM4 ############################################ -->
+
+<!-- ############################################# ARM5 ############################################ -->
+ <link name="Arm5_link">
+ <gravity>true</gravity>
+ <pose>-0.162293160669 -0.0937 0.009875004 0 0 3.66519142919</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm5">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm5_joint' type='fixed'>
+ <child>Arm5_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM5 ############################################ -->
+
+<!-- ############################################# ARM6 ############################################ -->
+ <link name="Arm6_link">
+ <gravity>true</gravity>
+ <pose>-0.162293160669 0.0937 0.009875004 0 0 2.61799387799</pose>
+
+ <inertial>
+ <mass>0.01</mass>
+ <inertia>
+ <ixx>0.001</ixx>
+ <ixy>0</ixy>
+ <ixz>0</ixz>
+ <iyy>0.001</iyy>
+ <iyz>0</iyz>
+ <izz>0.001</izz>
+ </inertia>
+ </inertial>
+
+ <visual name="visual_Arm6">
+ <geometry>
+ <mesh>
+ <scale>0.001 0.001 0.001</scale>
+ <uri>model://f550_amazing/meshes/Arm_CG.STL</uri>
+ </mesh>
+ </geometry>
+ <material>
+ <script>
+ <name>Gazebo/DarkGrey</name>
+ <uri>file://media/materials/scripts/gazebo.material</uri>
+ </script>
+ </material>
+ </visual>
+ </link>
+
+ <joint name='Lower_Arm6_joint' type='fixed'>
+ <child>Arm6_link</child>
+ <parent>base_link</parent>
+ </joint>
+<!-- ############################################ /ARM6 ############################################ -->
+
+ </model>
+</sdf>
diff --git a/simulation_control/src/models/f550_amazing_dead_blue/model.config b/simulation_control/src/models/f550_amazing_dead_blue/model.config
new file mode 100644
index 0000000000000000000000000000000000000000..6ea0dd4143d29e9590ff309adb0801d62a4bb6af
--- /dev/null
+++ b/simulation_control/src/models/f550_amazing_dead_blue/model.config
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+ <name>f550_amazing_dead_blue</name>
+ <version>1.0</version>
+ <sdf version="1.5">f550_amazing_dead_blue.sdf</sdf>
+
+ <author>
+ <name>Anders Bogga</name>
+ <email>andorsk@gmail.com</email>
+ </author>
+
+ <description>
+ f550_amazing_dead_blue
+ </description>
+</model>
diff --git a/simulation_control/src/models/landing_strip/landing_strip.sdf b/simulation_control/src/models/landing_strip/landing_strip.sdf
new file mode 100644
index 0000000000000000000000000000000000000000..c444768d46eea6e2baf007dbf4f9d09cdeb13b0e
--- /dev/null
+++ b/simulation_control/src/models/landing_strip/landing_strip.sdf
@@ -0,0 +1,135 @@
+<?xml version='1.0'?>
+<sdf version='1.6'>
+
+ <model name='landing_strip'>
+ <static>1</static>
+ <pose>0 0 0 0 0 0</pose>
+ <link name='base_link'>
+
+ <collision name='landing_strip_collision'>
+ <pose>0 0 0.05 0 0 0</pose>
+ <geometry>
+ <box>
+ <size>230 20 0.1</size>
+ </box>
+ </geometry>
+ <surface>
+ <friction>
+ <ode>
+ <mu>50</mu>
+ <mu2>50</mu2>
+ </ode>
+ </friction>
+ <contact>
+ <ode>
+ <min_depth>0.001</min_depth>
+ </ode>
+ </contact>
+ </surface>
+ <max_contacts>10</max_contacts>
+ </collision>
+
+ <visual name='landing_strip_visual'>
+ <pose>0 0 0.05 0 0 0</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>230 20 0.1</size>
+ </box>
+ </geometry>
+ <material>
+ <script>
+ <uri>model://asphalt_plane/materials/scripts</uri>
+ <uri>model://asphalt_plane/materials/textures</uri>
+ <name>vrc/asphalt</name>
+ </script>
+ </material>
+ </visual>
+
+ <visual name='white_line_long_visual'>
+ <pose>0 0 0.1 0 0 0</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>195 0.25 0.001</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <visual name='white_line_short_visual'>
+ <pose>0 0 0.1 0 0 0</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>0.25 15 0.001</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <visual name='white_line1_visual'>
+ <pose>105 0 0.1 0 0 0.785398</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>20 0.25 0.001</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <visual name='white_line2_visual'>
+ <pose>105 0 0.1 0 0 -0.785398</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>20 0.25 0.001</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <visual name='white_line3_visual'>
+ <pose>-105 0 0.1 0 0 0.785398</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>20 0.25 0.001</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <visual name='white_line4_visual'>
+ <pose>-105 0 0.1 0 0 -0.785398</pose>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <box>
+ <size>20 0.25 0.001</size>
+ </box>
+ </geometry>
+ </visual>
+
+ <self_collide>0</self_collide>
+ <kinematic>0</kinematic>
+ </link>
+
+ <link name='world'>
+ <collision name='world_collision'>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>model://landing_strip/meshes/Timpa.dae</uri>
+ </mesh>
+ </geometry>
+ </collision>
+
+ <visual name='world_visual'>
+ <cast_shadows>0</cast_shadows>
+ <geometry>
+ <mesh>
+ <scale>1 1 1</scale>
+ <uri>model://landing_strip/meshes/Timpa.dae</uri>
+ </mesh>
+ </geometry>
+ </visual>
+ </link>
+
+ </model>
+</sdf>
diff --git a/simulation_control/src/models/landing_strip/meshes/Timpa.dae b/simulation_control/src/models/landing_strip/meshes/Timpa.dae
new file mode 100644
index 0000000000000000000000000000000000000000..069b74f2a575260e8df1980008836535360e45d2
--- /dev/null
+++ b/simulation_control/src/models/landing_strip/meshes/Timpa.dae
@@ -0,0 +1,419 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
+<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
+ <asset>
+ <contributor>
+ <authoring_tool>SketchUp 18.0.16975</authoring_tool>
+ </contributor>
+ <created>2018-02-23T16:43:34Z</created>
+ <modified>2018-02-23T16:43:34Z</modified>
+ <unit meter="0.0254" name="inch" />
+ <up_axis>Z_UP</up_axis>
+ </asset>
+ <library_cameras>
+ <camera id="ID1" name="skp_camera_Last_Saved_SketchUp_View">
+ <optics>
+ <technique_common>
+ <perspective>
+ <yfov>35</yfov>
+ <aspect_ratio>0</aspect_ratio>
+ <znear>1</znear>
+ <zfar>1000</zfar>
+ </perspective>
+ </technique_common>
+ </optics>
+ </camera>
+ </library_cameras>
+ <library_visual_scenes>
+ <visual_scene id="ID2">
+ <node name="SketchUp">
+ <instance_geometry url="#ID3">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="Material2" target="#ID4">
+ <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+ </instance_material>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ <instance_geometry url="#ID16">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="Material2" target="#ID17">
+ <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+ </instance_material>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ <instance_geometry url="#ID29">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="Material2" target="#ID30">
+ <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+ </instance_material>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ <instance_geometry url="#ID42">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="Material2" target="#ID43">
+ <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+ </instance_material>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ <instance_geometry url="#ID55">
+ <bind_material>
+ <technique_common>
+ <instance_material symbol="Material2" target="#ID57">
+ <bind_vertex_input semantic="UVSET0" input_semantic="TEXCOORD" input_set="0" />
+ </instance_material>
+ </technique_common>
+ </bind_material>
+ </instance_geometry>
+ <node name="skp_camera_Last_Saved_SketchUp_View">
+ <matrix>0.9991642 -0.0280184 0.02976323 -6164.761 0.0408764 0.684869 -0.7275188 -38488.58 2.775558e-17 0.7281274 0.6854419 36138.63 0 0 0 1</matrix>
+ <instance_camera url="#ID1" />
+ </node>
+ </node>
+ </visual_scene>
+ </library_visual_scenes>
+ <library_geometries>
+ <geometry id="ID3">
+ <mesh>
+ <source id="ID9">
+ <float_array id="ID13" count="39">-7630.094 2801.055 0 -189.5268 4774.499 0 -351.9197 5386.792 0 -7792.477 3413.359 0 -8157.775 3316.472 0 -7848.27 2149.405 0 -42.3274 4219.492 0 11433.77 8512.669 0 11596.18 7900.393 0 3885.624 -10590.6 0 -3920.619 -12660.77 0 -3915.512 -12680.03 0 15676.84 -7483.608 0</float_array>
+ <technique_common>
+ <accessor count="13" source="#ID13" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID10">
+ <float_array id="ID14" count="39">0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1</float_array>
+ <technique_common>
+ <accessor count="13" source="#ID14" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID12">
+ <float_array id="ID15" count="26">0.01864551 0.96171 0.3984251 0.9617103 0.3984245 0.9999867 0.01864549 0.9999868 1.831868e-15 0.9999868 1.665335e-15 0.9270301 0.3984257 0.9270151 0.9999865 0.9999865 0.9999881 0.9617108 0.398441 0.001188706 2.164935e-15 0.001203656 2.164935e-15 3.885781e-15 1.000028 3.885781e-15</float_array>
+ <technique_common>
+ <accessor count="13" source="#ID15" stride="2">
+ <param name="S" type="float" />
+ <param name="T" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="ID11">
+ <input semantic="POSITION" source="#ID9" />
+ <input semantic="NORMAL" source="#ID10" />
+ </vertices>
+ <polylist count="4" material="Material2">
+ <input offset="0" semantic="VERTEX" source="#ID11" />
+ <input offset="1" semantic="TEXCOORD" source="#ID12" />
+ <vcount>4 6 4 7</vcount>
+ <p>0 0 1 1 2 2 3 3 4 4 5 5 6 6 1 1 0 0 3 3 7 7 2 2 1 1 8 8 1 1 6 6 9 9 10 10 11 11 12 12 8 8</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="ID16">
+ <mesh>
+ <source id="ID22">
+ <float_array id="ID26" count="33">-351.9197 5386.792 0 -356.9629 5405.806 0 -7797.58 3432.601 0 -7792.477 3413.359 0 -11872.2 18796.92 0 -11725.08 18242.17 0 -4284.748 20215.29 0 11433.77 8512.669 0 11596.18 7900.393 0 11961.47 7997.28 0 7718.555 23992.92 0</float_array>
+ <technique_common>
+ <accessor count="11" source="#ID26" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID23">
+ <float_array id="ID27" count="33">0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1</float_array>
+ <technique_common>
+ <accessor count="11" source="#ID27" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID25">
+ <float_array id="ID28" count="22">0.379779 0.03827636 0.379779 0.03946501 1.831868e-15 0.03947971 1.94289e-15 0.0382768 1.221245e-15 0.9999468 9.436896e-16 0.9652677 0.3797644 0.9652529 0.981341 0.03827565 0.9813426 -1.498801e-15 0.9999881 -1.498801e-15 0.9999465 0.9999465</float_array>
+ <technique_common>
+ <accessor count="11" source="#ID28" stride="2">
+ <param name="S" type="float" />
+ <param name="T" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="ID24">
+ <input semantic="POSITION" source="#ID22" />
+ <input semantic="NORMAL" source="#ID23" />
+ </vertices>
+ <polylist count="3" material="Material2">
+ <input offset="0" semantic="VERTEX" source="#ID24" />
+ <input offset="1" semantic="TEXCOORD" source="#ID25" />
+ <vcount>4 9 4</vcount>
+ <p>0 0 1 1 2 2 3 3 4 4 5 5 6 6 1 1 0 0 7 7 8 8 9 9 10 10 5 5 2 2 1 1 6 6</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="ID29">
+ <mesh>
+ <source id="ID35">
+ <float_array id="ID39" count="30">-15706.89 -15786.42 0 -3920.619 -12660.77 0 -7848.27 2149.405 0 -19634.09 -976.1221 0 -19948.67 210.2083 0 3885.624 -10590.6 0 -42.3274 4219.492 0 -8157.775 3316.472 0 -7792.477 3413.359 0 -7797.58 3432.601 0</float_array>
+ <technique_common>
+ <accessor count="10" source="#ID39" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID36">
+ <float_array id="ID40" count="30">0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1</float_array>
+ <technique_common>
+ <accessor count="10" source="#ID40" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID38">
+ <float_array id="ID41" count="20">6.883383e-15 3.108624e-15 0.6015872 3.108624e-15 0.601564 0.9258265 -7.634461e-09 0.9258267 6.550316e-15 0.999987 1.000028 3.219647e-15 0.9999897 0.9258264 0.6015622 0.9987831 0.6202077 0.9987838 0.6202076 0.9999867</float_array>
+ <technique_common>
+ <accessor count="10" source="#ID41" stride="2">
+ <param name="S" type="float" />
+ <param name="T" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="ID37">
+ <input semantic="POSITION" source="#ID35" />
+ <input semantic="NORMAL" source="#ID36" />
+ </vertices>
+ <polylist count="2" material="Material2">
+ <input offset="0" semantic="VERTEX" source="#ID37" />
+ <input offset="1" semantic="TEXCOORD" source="#ID38" />
+ <vcount>4 10</vcount>
+ <p>0 0 1 1 2 2 3 3 4 4 3 3 2 2 1 1 5 5 6 6 2 2 7 7 8 8 9 9</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="ID42">
+ <mesh>
+ <source id="ID48">
+ <float_array id="ID52" count="15">-23875.7 15019.9 0 -21912.18 7615.054 0 -19948.67 210.2083 0 -7797.58 3432.601 0 -11725.08 18242.17 0</float_array>
+ <technique_common>
+ <accessor count="5" source="#ID52" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID49">
+ <float_array id="ID53" count="15">0 0 1 0 0 1 0 0 1 0 0 1 0 0 1</float_array>
+ <technique_common>
+ <accessor count="5" source="#ID53" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID51">
+ <float_array id="ID54" count="10">5.662137e-15 0.9999484 -3.816949e-09 0.5370544 -7.633904e-09 0.07416027 0.6202076 0.07416026 0.6201838 0.9999482</float_array>
+ <technique_common>
+ <accessor count="5" source="#ID54" stride="2">
+ <param name="S" type="float" />
+ <param name="T" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="ID50">
+ <input semantic="POSITION" source="#ID48" />
+ <input semantic="NORMAL" source="#ID49" />
+ </vertices>
+ <polylist count="1" material="Material2">
+ <input offset="0" semantic="VERTEX" source="#ID50" />
+ <input offset="1" semantic="TEXCOORD" source="#ID51" />
+ <vcount>5</vcount>
+ <p>0 0 1 1 2 2 3 3 4 4</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ <geometry id="ID55">
+ <mesh>
+ <source id="ID58">
+ <float_array id="ID61" count="231">-3920.619 -12660.77 -196.8504 -15706.89 -15786.42 -196.8504 -19634.09 -976.1221 -196.8504 -7848.27 2149.405 -196.8504 -3920.619 -12660.77 -196.8504 -3920.619 -12660.77 0 -15706.89 -15786.42 0 -15706.89 -15786.42 -196.8504 -15706.89 -15786.42 -196.8504 -15706.89 -15786.42 0 -19634.09 -976.1221 0 -19634.09 -976.1221 -196.8504 -19948.67 210.2083 -196.8504 -7797.58 3432.601 -196.8504 -7792.477 3413.359 -196.8504 -8157.775 3316.472 -196.8504 -42.3274 4219.492 -196.8504 3885.624 -10590.6 -196.8504 -3915.512 -12680.03 -196.8504 -3915.512 -12680.03 0 -3920.619 -12660.77 0 -3920.619 -12660.77 -196.8504 -19634.09 -976.1221 -196.8504 -19634.09 -976.1221 0 -19948.67 210.2083 0 -19948.67 210.2083 -196.8504 -21912.18 7615.054 -196.8504 -23875.7 15019.9 -196.8504 -11725.08 18242.17 -196.8504 -356.9629 5405.806 -196.8504 -351.9197 5386.792 -196.8504 -7630.094 2801.055 -196.8504 -189.5268 4774.499 -196.8504 11596.18 7900.393 -196.8504 15676.84 -7483.608 -196.8504 -3915.512 -12680.03 -196.8504 15676.84 -7483.608 -196.8504 15676.84 -7483.608 0 -3915.512 -12680.03 0 -3915.512 -12680.03 -196.8504 -21912.18 7615.054 0 -21912.18 7615.054 -196.8504 -19948.67 210.2083 -196.8504 -19948.67 210.2083 0 -21912.18 7615.054 -196.8504 -21912.18 7615.054 0 -23875.7 15019.9 0 -23875.7 15019.9 -196.8504 -23875.7 15019.9 -196.8504 -23875.7 15019.9 0 -11725.08 18242.17 0 -11725.08 18242.17 -196.8504 -4284.748 20215.29 -196.8504 -11872.2 18796.92 -196.8504 7718.555 23992.92 -196.8504 11961.47 7997.28 -196.8504 11433.77 8512.669 -196.8504 11596.18 7900.393 -196.8504 11596.18 7900.393 0 15676.84 -7483.608 0 15676.84 -7483.608 -196.8504 -11725.08 18242.17 -196.8504 -11725.08 18242.17 0 -11872.2 18796.92 0 -11872.2 18796.92 -196.8504 -11872.2 18796.92 -196.8504 -11872.2 18796.92 0 7718.555 23992.92 0 7718.555 23992.92 -196.8504 11961.47 7997.28 0 11961.47 7997.28 -196.8504 7718.555 23992.92 -196.8504 7718.555 23992.92 0 11961.47 7997.28 -196.8504 11961.47 7997.28 0 11596.18 7900.393 0 11596.18 7900.393 -196.8504</float_array>
+ <technique_common>
+ <accessor count="77" source="#ID61" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <source id="ID59">
+ <float_array id="ID62" count="231">0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.2563335 -0.9665884 -7.727933e-18 0.2563335 -0.9665884 -7.727933e-18 0.2563335 -0.9665884 -7.727933e-18 0.2563335 -0.9665884 -7.727933e-18 -0.966595 -0.2563088 2.341444e-17 -0.966595 -0.2563088 2.341444e-17 -0.966595 -0.2563088 2.341444e-17 -0.966595 -0.2563088 2.341444e-17 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 -0.9665871 -0.2563384 -9.216696e-15 -0.9665871 -0.2563384 -9.216696e-15 -0.9665871 -0.2563384 -9.216696e-15 -0.9665871 -0.2563384 -9.216696e-15 -0.966595 -0.2563086 -1.843903e-14 -0.966595 -0.2563086 -1.843903e-14 -0.966595 -0.2563086 -1.843903e-14 -0.966595 -0.2563086 -1.843903e-14 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.2563631 -0.9665805 -1.850199e-14 0.2563631 -0.9665805 -1.850199e-14 0.2563631 -0.9665805 -1.850199e-14 0.2563631 -0.9665805 -1.850199e-14 -0.966595 -0.2563085 -1.843341e-14 -0.966595 -0.2563085 -1.843341e-14 -0.966595 -0.2563085 -1.843341e-14 -0.966595 -0.2563085 -1.843341e-14 -0.966595 -0.2563085 2.341689e-17 -0.966595 -0.2563085 2.341689e-17 -0.966595 -0.2563085 2.341689e-17 -0.966595 -0.2563085 2.341689e-17 -0.256333 0.9665885 2.318848e-17 -0.256333 0.9665885 2.318848e-17 -0.256333 0.9665885 2.318848e-17 -0.256333 0.9665885 2.318848e-17 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0.966574 0.2563876 9.203793e-15 0.966574 0.2563876 9.203793e-15 0.966574 0.2563876 9.203793e-15 0.966574 0.2563876 9.203793e-15 -0.9665869 -0.2563391 -1.386092e-14 -0.9665869 -0.2563391 -1.386092e-14 -0.9665869 -0.2563391 -1.386092e-14 -0.9665869 -0.2563391 -1.386092e-14 -0.2563635 0.9665804 0 -0.2563635 0.9665804 0 -0.2563635 0.9665804 0 -0.2563635 0.9665804 0 0.9665739 0.2563883 -1.511539e-17 0.9665739 0.2563883 -1.511539e-17 0.9665739 0.2563883 -1.511539e-17 0.9665739 0.2563883 -1.511539e-17 0.2563638 -0.9665804 -9.240491e-15 0.2563638 -0.9665804 -9.240491e-15 0.2563638 -0.9665804 -9.240491e-15 0.2563638 -0.9665804 -9.240491e-15</float_array>
+ <technique_common>
+ <accessor count="77" source="#ID62" stride="3">
+ <param name="X" type="float" />
+ <param name="Y" type="float" />
+ <param name="Z" type="float" />
+ </accessor>
+ </technique_common>
+ </source>
+ <vertices id="ID60">
+ <input semantic="POSITION" source="#ID58" />
+ <input semantic="NORMAL" source="#ID59" />
+ </vertices>
+ <polylist count="23" material="Material2">
+ <input offset="0" semantic="VERTEX" source="#ID60" />
+ <vcount>4 4 4 10 4 4 5 4 6 7 4 4 4 4 4 9 4 4 4 4 4 4 4</vcount>
+ <p>0 1 2 3 4 5 6 7 8 9 10 11 2 12 13 14 15 3 16 17 0 3 18 19 20 21 22 23 24 25 26 27 28 13 12 29 30 14 13 3 15 14 31 32 16 16 32 33 34 35 0 17 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 13 28 52 29 28 53 54 55 33 56 30 29 52 32 31 14 30 30 56 33 32 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76</p>
+ </polylist>
+ </mesh>
+ </geometry>
+ </library_geometries>
+ <library_materials>
+ <material id="ID4" name="Location_Snapshot_2">
+ <instance_effect url="#ID5" />
+ </material>
+ <material id="ID17" name="Location_Snapshot_4">
+ <instance_effect url="#ID18" />
+ </material>
+ <material id="ID30" name="Location_Snapshot_1">
+ <instance_effect url="#ID31" />
+ </material>
+ <material id="ID43" name="Location_Snapshot_3">
+ <instance_effect url="#ID44" />
+ </material>
+ <material id="ID57" name="material">
+ <instance_effect url="#ID56" />
+ </material>
+ </library_materials>
+ <library_effects>
+ <effect id="ID5">
+ <profile_COMMON>
+ <newparam sid="ID7">
+ <surface type="2D">
+ <init_from>ID6</init_from>
+ </surface>
+ </newparam>
+ <newparam sid="ID8">
+ <sampler2D>
+ <source>ID7</source>
+ </sampler2D>
+ </newparam>
+ <technique sid="COMMON">
+ <lambert>
+ <diffuse>
+ <texture texture="ID8" texcoord="UVSET0" />
+ </diffuse>
+ </lambert>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ <effect id="ID18">
+ <profile_COMMON>
+ <newparam sid="ID20">
+ <surface type="2D">
+ <init_from>ID19</init_from>
+ </surface>
+ </newparam>
+ <newparam sid="ID21">
+ <sampler2D>
+ <source>ID20</source>
+ </sampler2D>
+ </newparam>
+ <technique sid="COMMON">
+ <lambert>
+ <diffuse>
+ <texture texture="ID21" texcoord="UVSET0" />
+ </diffuse>
+ </lambert>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ <effect id="ID31">
+ <profile_COMMON>
+ <newparam sid="ID33">
+ <surface type="2D">
+ <init_from>ID32</init_from>
+ </surface>
+ </newparam>
+ <newparam sid="ID34">
+ <sampler2D>
+ <source>ID33</source>
+ </sampler2D>
+ </newparam>
+ <technique sid="COMMON">
+ <lambert>
+ <diffuse>
+ <texture texture="ID34" texcoord="UVSET0" />
+ </diffuse>
+ </lambert>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ <effect id="ID44">
+ <profile_COMMON>
+ <newparam sid="ID46">
+ <surface type="2D">
+ <init_from>ID45</init_from>
+ </surface>
+ </newparam>
+ <newparam sid="ID47">
+ <sampler2D>
+ <source>ID46</source>
+ </sampler2D>
+ </newparam>
+ <technique sid="COMMON">
+ <lambert>
+ <diffuse>
+ <texture texture="ID47" texcoord="UVSET0" />
+ </diffuse>
+ </lambert>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ <effect id="ID56">
+ <profile_COMMON>
+ <technique sid="COMMON">
+ <lambert>
+ <diffuse>
+ <color>1 1 1 1</color>
+ </diffuse>
+ </lambert>
+ </technique>
+ </profile_COMMON>
+ </effect>
+ </library_effects>
+ <library_images>
+ <image id="ID6">
+ <init_from>Timpa/Location_Snapshot_2.jpg</init_from>
+ </image>
+ <image id="ID19">
+ <init_from>Timpa/Location_Snapshot_4.jpg</init_from>
+ </image>
+ <image id="ID32">
+ <init_from>Timpa/Location_Snapshot_1.jpg</init_from>
+ </image>
+ <image id="ID45">
+ <init_from>Timpa/Location_Snapshot_3.jpg</init_from>
+ </image>
+ </library_images>
+ <scene>
+ <instance_visual_scene url="#ID2" />
+ </scene>
+</COLLADA>
diff --git a/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_1.jpg b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_1.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..170bb859a399927e7d51d77bdb2887856eb3d70b
Binary files /dev/null and b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_1.jpg differ
diff --git a/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_2.jpg b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_2.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..e6c1b3ad4235327813d30a37b71a94c49beab9a6
Binary files /dev/null and b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_2.jpg differ
diff --git a/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_3.jpg b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_3.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..a76760bde26e1984ab3e3c64560818059d03374c
Binary files /dev/null and b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_3.jpg differ
diff --git a/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_4.jpg b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_4.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..b164254d1a3d1a4254e1f7b9509f883a8ebe85e0
Binary files /dev/null and b/simulation_control/src/models/landing_strip/meshes/Timpa/Location_Snapshot_4.jpg differ
diff --git a/simulation_control/src/models/landing_strip/model.config b/simulation_control/src/models/landing_strip/model.config
new file mode 100644
index 0000000000000000000000000000000000000000..be537ebc82472f0dd9fba0cab200a97adae41ba4
--- /dev/null
+++ b/simulation_control/src/models/landing_strip/model.config
@@ -0,0 +1,13 @@
+<?xml version="1.0" ?>
+<model>
+ <name>landing_strip</name>
+ <version>1.0</version>
+ <sdf version="1.6">landing_strip.sdf</sdf>
+ <author>
+ <name>Anders Bogga</name>
+ <email>andorsk@gmail.com</email>
+ </author>
+ <description>
+ landing_strip
+ </description>
+</model>
diff --git a/simulation_control/src/scripts/PID.py b/simulation_control/src/scripts/PID.py
new file mode 100644
index 0000000000000000000000000000000000000000..959fb2512bd74c6a79a3eb19dcd6d3fa12dd8b34
--- /dev/null
+++ b/simulation_control/src/scripts/PID.py
@@ -0,0 +1,77 @@
+#!/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
diff --git a/simulation_control/src/scripts/PID.pyc b/simulation_control/src/scripts/PID.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..80020d56c3dcb0c7b0b1a54961e8627fcc269165
Binary files /dev/null and b/simulation_control/src/scripts/PID.pyc differ
diff --git a/simulation_control/src/scripts/VelocityController.py b/simulation_control/src/scripts/VelocityController.py
new file mode 100644
index 0000000000000000000000000000000000000000..1fd182b7cd3a13ebffa395eed8b8af7beb2803e3
--- /dev/null
+++ b/simulation_control/src/scripts/VelocityController.py
@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+import rospy
+from mavros_msgs.msg import State
+from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, Vector3, Twist, TwistStamped
+import math
+import numpy
+from std_msgs.msg import Header
+from PID import PID
+
+
+class VelocityController:
+ target = PoseStamped()
+ output = TwistStamped()
+
+ def __init__(self):
+ self.X = PID()
+ self.Y = PID()
+ self.Z = PID()
+ self.lastTime = rospy.get_time()
+ self.target = None
+
+ def setTarget(self, target):
+ self.target = target
+
+ def update(self, state):
+ if (self.target is None):
+ rospy.logwarn("Target position for velocity controller is none.")
+ return None
+ # simplify variables a bit
+ time = state.header.stamp.to_sec()
+ position = state.pose.position
+ orientation = state.pose.orientation
+ # create output structure
+ output = TwistStamped()
+ output.header = state.header
+ # output velocities
+ linear = Vector3()
+ angular = Vector3()
+ # Control in X vel
+ linear.x = self.X.update(self.target.position.x, position.x, time)
+ # Control in Y vel
+ linear.y = self.Y.update(self.target.position.y, position.y, time)
+ # Control in Z vel
+ linear.z = self.Z.update(self.target.position.z, position.z, time)
+ # Control yaw (no x, y angular)
+ # TODO
+ output.twist = Twist()
+ output.twist.linear = linear
+ return output
+
+ def stop(self):
+ setTarget(self.current)
+ update(self.current)
+ def set_x_pid(self, kp, ki, kd, output):
+ self.X.setKp(kp)
+ self.X.setKi(ki)
+ self.X.setKd(kd)
+ self.X.setMaxO(output)
+ def set_y_pid(self, kp, ki, kd, output):
+ self.Y.setKp(kp)
+ self.Y.setKi(ki)
+ self.Y.setKd(kd)
+ self.Y.setMaxO(output)
+ def set_z_pid(self, kp, ki, kd, output):
+ self.Z.setKp(kp)
+ self.Z.setKi(ki)
+ self.Z.setKd(kd)
+ self.Z.setMaxO(output)
diff --git a/simulation_control/src/scripts/VelocityController.pyc b/simulation_control/src/scripts/VelocityController.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..a713af9f6998098a608f7ae3258bb4049e933b08
Binary files /dev/null and b/simulation_control/src/scripts/VelocityController.pyc differ
diff --git a/simulation_control/src/scripts/action_controller.py b/simulation_control/src/scripts/action_controller.py
new file mode 100644
index 0000000000000000000000000000000000000000..7d4c4a84e1ccd169ab460a292174a362b0963c51
--- /dev/null
+++ b/simulation_control/src/scripts/action_controller.py
@@ -0,0 +1,178 @@
+#!/usr/bin/env python
+
+import roslib
+roslib.load_manifest('simulation_control')
+import rospy
+import actionlib
+import mavros_state
+import time
+
+from simulation_control.msg import center_on_objectAction, center_on_objectGoal, descend_on_objectAction, descend_on_objectGoal, detect_objectAction, detect_objectGoal, goto_positionAction, goto_positionGoal, short_grippersAction, short_grippersGoal, long_grippersAction, long_grippersGoal
+
+from std_msgs.msg import Float32
+
+if __name__ == '__main__':
+ rospy.init_node('action_controller')
+ mv_state = mavros_state.mavros_state()
+ print 'Setting offboard'
+ mv_state.set_mode('OFFBOARD')
+ print 'Arming vehicle'
+ mv_state.arm(True)
+ rospy.loginfo("Taking off")
+ goto_position_client = actionlib.SimpleActionClient('goto_position', goto_positionAction)
+ goto_position_client.wait_for_server()
+ goto_position_goal = goto_positionGoal()
+ goto_position_goal.destination.pose.position.z = 5
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+ rospy.loginfo("Takeoff succeded")
+
+ # Close legs
+ long_grippers_client = actionlib.SimpleActionClient('long_grippers', long_grippersAction)
+ long_grippers_client.wait_for_server()
+ rospy.loginfo("Moving legs to pickup position")
+ long_grippers_goal = long_grippersGoal(grip_rad_goal=Float32(0.5000000000))
+ long_grippers_client.send_goal(long_grippers_goal)
+ long_grippers_client.wait_for_result()
+ if long_grippers_client.get_result().goal_reached.data:
+ rospy.loginfo("Legs moved to pickup position")
+ else:
+ rospy.loginfo("Error with moving legs to pickup position")
+ # /Close legs
+
+ goto_position_goal.destination.pose.position.x = -105
+ goto_position_goal.destination.pose.position.y = 0
+ goto_position_goal.destination.pose.position.z = 3
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+ rospy.loginfo("Arrived at search position.")
+
+ rospy.loginfo("Searching...")
+ detect_object_client = actionlib.SimpleActionClient('detect_object', detect_objectAction)
+ detect_object_client.wait_for_server()
+ detect_object_goal = detect_objectGoal()
+ detect_object_client.send_goal(detect_object_goal)
+ detect_object_client.wait_for_result()
+ print(detect_object_client.get_result())
+
+ rospy.loginfo("Going to detected position")
+ goto_position_goal.destination.pose.position.x = detect_object_client.get_result().detected_position.pose.position.x
+ goto_position_goal.destination.pose.position.y = detect_object_client.get_result().detected_position.pose.position.y
+ goto_position_goal.destination.pose.position.z = detect_object_client.get_result().detected_position.pose.position.z
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+ rospy.loginfo("Is at position (x,y,z)=({}, {}, {})".format(detect_object_client.get_result().detected_position.pose.position.x,
+ detect_object_client.get_result().detected_position.pose.position.y,
+ detect_object_client.get_result().detected_position.pose.position.z))
+
+ rospy.loginfo("Descending on object")
+ descend_on_object_client = actionlib.SimpleActionClient('descend_on_object', descend_on_objectAction)
+ descend_on_object_client.wait_for_server()
+ rospy.loginfo("Descending server started")
+ descend_on_object_goal = descend_on_objectGoal()
+ descend_on_objectGoal = 2.0
+ descend_on_object_client.send_goal(descend_on_object_goal)
+ descend_on_object_client.wait_for_result()
+ if descend_on_object_client.get_result().position_reached.data:
+ print("landing")
+ mv_state.arm(False)
+ else:
+ rospy.loginfo("Couldnt land exiting")
+
+ time.sleep(3)
+
+ # Grip object
+ short_grippers_client = actionlib.SimpleActionClient('short_grippers', short_grippersAction)
+ short_grippers_client.wait_for_server()
+ rospy.loginfo("Closing short grippers")
+ short_grippers_goal = short_grippersGoal(grip_rad_goal=Float32(1.22000000000))
+ short_grippers_client.send_goal(short_grippers_goal)
+ short_grippers_client.wait_for_result()
+ if short_grippers_client.get_result().goal_reached.data:
+ rospy.loginfo("Short grippers closed")
+ else:
+ rospy.loginfo("Error when closing short grippers")
+ # /Grip object
+
+ time.sleep(3)
+
+ print 'Setting offboard'
+ mv_state.set_mode('OFFBOARD')
+ print 'Arming vehicle'
+ mv_state.arm(True)
+
+ time.sleep(1)
+
+ rospy.loginfo("Lifting")
+ goto_position_goal.destination.pose.position.x = -105
+ goto_position_goal.destination.pose.position.y = 0
+ goto_position_goal.destination.pose.position.z = 1.5
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+
+ time.sleep(1)
+
+ # Close legs
+ rospy.loginfo("Closing legs around object")
+ long_grippers_goal = long_grippersGoal(grip_rad_goal=Float32(1.57000000000))
+ long_grippers_client.send_goal(long_grippers_goal)
+ long_grippers_client.wait_for_result()
+ if long_grippers_client.get_result().goal_reached.data:
+ print("Legs closed")
+ else:
+ rospy.loginfo("Error with closing legs")
+ # /Close legs
+
+ time.sleep(1)
+
+ rospy.loginfo("Going to drop off position")
+ goto_position_goal.destination.pose.position.x = 0
+ goto_position_goal.destination.pose.position.y = 105
+ goto_position_goal.destination.pose.position.z = 5
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+
+ time.sleep(3)
+
+ rospy.loginfo("Deceding prior to drop off")
+ goto_position_goal.destination.pose.position.x = 0
+ goto_position_goal.destination.pose.position.y = 105
+ goto_position_goal.destination.pose.position.z = 0.5
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+
+ time.sleep(3)
+
+ # Open legs
+ rospy.loginfo("Opening legs around object")
+ long_grippers_goal = long_grippersGoal(grip_rad_goal=Float32(0.00000000000))
+ long_grippers_client.send_goal(long_grippers_goal)
+ long_grippers_client.wait_for_result()
+ if long_grippers_client.get_result().goal_reached.data:
+ print("Legs opened")
+ else:
+ rospy.loginfo("Error with leg opening")
+ # /Open legs
+
+ time.sleep(1)
+
+ # Release object
+ rospy.loginfo("short_grippers")
+ short_grippers_goal = short_grippersGoal(grip_rad_goal=Float32(0.00000000000))
+ short_grippers_client.send_goal(short_grippers_goal)
+ short_grippers_client.wait_for_result()
+ if short_grippers_client.get_result().goal_reached.data:
+ print("Grip position reached")
+ else:
+ rospy.loginfo("Grip position not reached")
+ # /Release object
+
+ time.sleep(3)
+
+ rospy.loginfo("Going Home")
+ goto_position_goal.destination.pose.position.x = 0
+ goto_position_goal.destination.pose.position.y = -105
+ goto_position_goal.destination.pose.position.z = 5
+ goto_position_client.send_goal(goto_position_goal)
+ goto_position_client.wait_for_result()
+ mv_state.land(0.0)
diff --git a/simulation_control/src/scripts/color_detection.py b/simulation_control/src/scripts/color_detection.py
new file mode 100644
index 0000000000000000000000000000000000000000..6cff8a2ce30d7b1e3fe017854f4dc790c331027f
--- /dev/null
+++ b/simulation_control/src/scripts/color_detection.py
@@ -0,0 +1,131 @@
+#!/usr/bin/env python
+# Python libs
+import sys, time
+
+# numpy and scipy
+import numpy as np
+from scipy.ndimage import filters
+from geometry_msgs.msg import Point
+
+# OpenCV
+import cv2
+
+# Ros libraries
+import roslib
+import rospy
+
+# Ros Messages
+from sensor_msgs.msg import CompressedImage, Image
+from cv_bridge import CvBridge, CvBridgeError
+# We do not use cv_bridge it does not support CompressedImage in python
+# from cv_bridge import CvBridge, CvBridgeError
+
+VERBOSE=False
+
+class image_feature:
+
+ def __init__(self):
+ '''Initialize ros publisher, ros subscriber'''
+
+ # topic where we publish
+ self.image_pub = rospy.Publisher("/color_detection/image",
+ Image, queue_size=10)
+ self.bridge = CvBridge()
+ #topic where the coordinates go
+ self.cam_pose_pub = rospy.Publisher("/color_detection/cam_point", Point, queue_size=1)
+ self.cam_pose = Point()
+ # subscribed Topic
+ self.subscriber = rospy.Subscriber("/uav_camera/image_raw_down", Image, self.callback, queue_size=1)
+ if VERBOSE :
+ print "subscribed to /uav_camera/image_raw_down"
+
+
+ def callback(self, ros_data):
+ '''Callback function of subscribed topic.
+ Here images get converted and features detected'''
+ if VERBOSE :
+ print 'received image of type: "%s"' % ros_data.format
+
+ ###Set boundarys###
+ lower_range = np.array([0, 255, 255], dtype=np.uint8) #The object color is 255,153,0
+ upper_range = np.array([0, 0, 255], dtype=np.uint8)
+
+ lower_blue = np.array([100, 150, 0])
+ upper_blue = np.array([140, 255, 255])
+
+ yellow_lr = np.array([20, 100, 100], dtype=np.uint8)
+ yellow_ur = np.array([30, 255, 255], dtype=np.uint8)
+
+ try:
+ cv_image = self.bridge.imgmsg_to_cv2(ros_data, "bgr8")
+ except CvBridgeError as e:
+ print(e)
+
+ hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
+ mask = cv2.inRange(hsv, lower_blue, upper_blue)
+ mask = cv2.erode(mask, None, iterations=2)
+ mask = cv2.dilate(mask, None, iterations=2)
+ output = cv2.bitwise_and(cv_image, cv_image, mask=mask)
+ image, contours, hierarchy = cv2.findContours(mask.copy(), cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
+
+ center = Point()
+ if len(contours) > 0:
+ c = max(contours, key=cv2.contourArea)
+ x, y, w, h = cv2.boundingRect(c)
+
+ M = cv2.moments(c)
+ center.x = float(M["m10"] / M["m00"])
+ center.y = float(M["m01"] / M["m00"])
+ self.find_relative_pose(center.x, center.y, w, h)
+ cv2.rectangle(cv_image, (x, y),(x+w, y+h), (0, 255, 255), 2)
+ else:
+ self.cam_pose = Point()
+ self.cam_pose.x = float("inf")
+ self.cam_pose.y = float("inf")
+ self.cam_pose.z = float("inf")
+ self.cam_pose_pub.publish(self.cam_pose)
+
+ try:
+ self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
+ except CvBridgeError as e:
+ print(e)
+
+ def find_relative_pose(self, x, y, w, h):
+ quad_3d = np.float32([[-0.0895, -0.0895, 0], [0.0895, -0.0895, 0], [0.0895, 0.0895, 0], [-0.0895, 0.0895, 0]])
+ quad = np.float32([[x - w / 2, y - h / 2], [x - w / 2, y + h / 2], [x + w / 2, y + h / 2], [x + w / 2, y - h / 2]])
+
+ K = np.float64([[1004.603218, 0 , 640.5],
+ [ 0 , 1004.603218, 360.5],
+ [ 0.0 , 0.0 , 1.0]])
+
+ dist_coef = np.zeros(4)
+ _ret, rvec, tvec = cv2.solvePnP(quad_3d, quad, K, dist_coef)
+ rmat = cv2.Rodrigues(rvec)[0]
+ cameraTranslatevector = -np.matrix(rmat).T * np.matrix(tvec)
+
+ T0 = np.zeros((4, 4))
+ T0[:3, :3] = rmat
+ T0[:4, 3] = [0, 0, 0, 1]
+ T0[:3, 3] = np.transpose(cameraTranslatevector)
+
+ p0 = np.array([-0.0895/2, -0.0895/2, 0, 1])
+ z0 = np.dot(T0, p0)
+
+ self.cam_pose.x = z0.item(0)
+ self.cam_pose.y = z0.item(1)
+ self.cam_pose.z = z0.item(2)
+ self.cam_pose_pub.publish(self.cam_pose)
+
+def main(args):
+ '''Initializes and cleanup ros node'''
+ ic = image_feature()
+ rospy.init_node('color_detection', anonymous=True)
+ rospy.Rate(20)
+ try:
+ rospy.spin()
+ except KeyboardInterrupt:
+ print "Shutting down ROS Image color detector module"
+ cv2.destroyAllWindows()
+
+if __name__ == '__main__':
+ main(sys.argv)
diff --git a/simulation_control/src/scripts/descend_on_object_server.py b/simulation_control/src/scripts/descend_on_object_server.py
new file mode 100644
index 0000000000000000000000000000000000000000..c97c6be40d6cf6590915a0ef04b34d22c25f4424
--- /dev/null
+++ b/simulation_control/src/scripts/descend_on_object_server.py
@@ -0,0 +1,117 @@
+#!/usr/bin/env python
+import rospy
+import actionlib
+import simulation_control.msg
+from geometry_msgs.msg import PoseStamped, Point
+from std_msgs.msg import Bool, String
+
+class descend_on_object_server():
+ def __init__(self):
+
+ #variables
+ self.local_pose = PoseStamped()
+ self.des_pose = PoseStamped()
+ self.object_pose = Point()
+
+ #publishers
+ self.mode_control = rospy.Publisher('/position_control/set_mode', String, queue_size=10)
+ self.vel_control = rospy.Publisher('/position_control/set_velocity', PoseStamped, queue_size=10)
+
+ #subscribers
+ rospy.Subscriber('/color_detection/cam_point', Point, self.get_cam_pos_callback)
+ rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
+ rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
+
+ self.rate = rospy.Rate(20)
+ self.result = simulation_control.msg.descend_on_objectResult()
+ self.action_server = actionlib.SimpleActionServer('descend_on_object',
+ simulation_control.msg.descend_on_objectAction,
+ execute_cb=self.execute_cb,
+ auto_start=False)
+ self.last_object_pose = Point()
+ self.action_server.start()
+
+
+ def execute_cb(self, goal):
+ rospy.loginfo("Starting to descend")
+ self.mode_control.publish('velctr')
+ rospy.sleep(0.1)
+
+ while self.local_pose.pose.position.z > 1.0:
+ self.rate.sleep()
+ print("x = ",self.local_pose.pose.position.x)
+ print("y = ",self.local_pose.pose.position.y)
+ print("z = ",self.local_pose.pose.position.z)
+ rospy.sleep(0.2)
+
+
+ if self.detected and (abs(self.object_pose.x) > 0.2 or abs(self.object_pose.y) > 0.2):
+ self.last_object_pose = self.object_pose
+ self.des_pose.pose.position.x = self.object_pose.x
+ self.des_pose.pose.position.y = self.object_pose.y
+ self.des_pose.pose.position.z = self.local_pose.pose.position.z
+ self.vel_control.publish(self.des_pose)
+ rospy.loginfo("Centering...")
+ while not self.target_reached:
+ rospy.sleep(2)
+
+ elif self.detected and abs(self.object_pose.x) < 0.2 and abs(self.object_pose.y) < 0.2:
+ self.des_pose.pose.position.x = 0
+ self.des_pose.pose.position.y = 0
+ self.des_pose.pose.position.z = self.local_pose.pose.position.z - 0.9
+ rospy.loginfo("Descending...")
+ self.vel_control.publish(self.des_pose)
+ while not self.target_reached:
+ rospy.sleep(2)
+
+ while self.local_pose.pose.position.z < 1.0 and self.local_pose.pose.position.z > 0.1:
+ self.rate.sleep()
+ print("x = ",self.local_pose.pose.position.x)
+ print("y = ",self.local_pose.pose.position.y)
+ print("z = ",self.local_pose.pose.position.z)
+ rospy.sleep(0.2)
+
+ if self.detected and (abs(self.object_pose.x) > 0.05 or abs(self.object_pose.y) > 0.05):
+ self.last_object_pose = self.object_pose
+ self.des_pose.pose.position.x = self.object_pose.x
+ self.des_pose.pose.position.y = self.object_pose.y
+ self.vel_control.publish(self.des_pose)
+ rospy.loginfo("Centering...")
+ while not self.target_reached:
+ rospy.sleep(2)
+
+ elif self.detected and abs(self.object_pose.x) < 0.05 and abs(self.object_pose.y) < 0.05:
+ self.des_pose.pose.position.x = 0
+ self.des_pose.pose.position.y = 0
+ self.des_pose.pose.position.z = self.local_pose.pose.position.z - 0.1
+ rospy.loginfo("Descending...")
+ self.vel_control.publish(self.des_pose)
+ while not self.target_reached:
+ rospy.sleep(2)
+
+ print("Landing")
+ self.vel_control.publish(self.des_pose)
+ self.rate.sleep()
+ self.result.position_reached.data = True
+ self.action_server.set_succeeded(self.result)
+
+ def get_cam_pos_callback(self, data):
+ if data.x != float("inf"):
+ self.detected = True
+ self.object_pose = data
+ else:
+ self.detected = False
+
+ def _local_pose_callback(self, data):
+ self.local_pose = data
+
+ def distance_reached_cb(self, data):
+ self.target_reached = data.data
+
+if __name__ == '__main__':
+ try:
+
+ rospy.init_node('descend_on_object_server')
+ descend_on_object_server()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/simulation_control/src/scripts/detect_object_server.py b/simulation_control/src/scripts/detect_object_server.py
new file mode 100644
index 0000000000000000000000000000000000000000..11c8a694a61eb651fe8560e7adce3acc88ecf001
--- /dev/null
+++ b/simulation_control/src/scripts/detect_object_server.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+import rospy
+import actionlib
+import simulation_control.msg
+
+from geometry_msgs.msg import PoseStamped, Point
+class detect_object_server():
+ def __init__(self):
+
+ #variables
+ self.local_pose = PoseStamped()
+ self.detected = False
+
+ #publishers
+
+ #subscribers
+ rospy.Subscriber('/color_detection/cam_point', Point, self.get_cam_pos_callback)
+ rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
+
+ self.rate = rospy.Rate(20)
+ self.result = simulation_control.msg.detect_objectResult()
+ self.action_server = actionlib.SimpleActionServer('detect_object', simulation_control.msg.detect_objectAction,
+ execute_cb=self.execute_cb, auto_start=False)
+ self.action_server.start()
+
+
+ def execute_cb(self, goal):
+ rospy.sleep(0.1)
+ while not self.detected:
+ self.rate.sleep()
+
+ rospy.loginfo("Target Detected")
+ self.result.detected_position = self.local_pose
+ self.action_server.set_succeeded(self.result)
+
+
+ def get_cam_pos_callback(self, data):
+ if data.x != float("inf"):
+ self.detected = True
+ self.object_pose = data
+ else:
+ self.detected = False
+
+
+ def _local_pose_callback(self, data):
+ self.local_pose = data
+
+if __name__ == '__main__':
+ try:
+
+ rospy.init_node('detect_object_server')
+ detect_object_server()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/simulation_control/src/scripts/goto_position_server.py b/simulation_control/src/scripts/goto_position_server.py
new file mode 100644
index 0000000000000000000000000000000000000000..d983d654874308954db1de7f3d2e86eb854d3e1d
--- /dev/null
+++ b/simulation_control/src/scripts/goto_position_server.py
@@ -0,0 +1,48 @@
+#!/usr/bin/env python
+import rospy
+import actionlib
+import simulation_control.msg
+from std_msgs.msg import Bool, String
+
+from geometry_msgs.msg import PoseStamped
+class goto_position_server():
+ def __init__(self):
+
+ #variables
+ self.target_reached = False
+ #publishers
+ self.pose_control = rospy.Publisher('/position_control/set_position', PoseStamped, queue_size=10)
+ self.mode_control = rospy.Publisher('/position_control/set_mode', String, queue_size=10)
+
+ #subscribers
+ rospy.Subscriber('/position_control/distance', Bool, self.distance_reached_cb)
+
+ self.rate = rospy.Rate(20)
+ #self.result = simulation_control.msg.goto_positionResult()
+ self.action_server = actionlib.SimpleActionServer('goto_position', simulation_control.msg.goto_positionAction,
+ execute_cb=self.execute_cb, auto_start=False)
+ self.action_server.start()
+
+
+ def execute_cb(self, goal):
+ self.mode_control.publish('posctr')
+ self.pose_control.publish(goal.destination)
+ rospy.sleep(0.1)
+ print(self.target_reached)
+ while not self.target_reached:
+ self.rate.sleep()
+
+ rospy.loginfo("Destination reached")
+ self.action_server.set_succeeded()
+
+
+ def distance_reached_cb(self, data):
+ self.target_reached = data.data
+
+if __name__ == '__main__':
+ try:
+
+ rospy.init_node('goto_position_server')
+ goto_position_server()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/simulation_control/src/scripts/long_grippers_server.py b/simulation_control/src/scripts/long_grippers_server.py
new file mode 100644
index 0000000000000000000000000000000000000000..486bf4fb9ecc713b7b9a734835d1fb2206cd7842
--- /dev/null
+++ b/simulation_control/src/scripts/long_grippers_server.py
@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+
+import roslib
+import rospy
+import actionlib
+import simulation_control.msg
+import time
+
+from std_msgs.msg import Float32
+
+class long_grippers_server():
+ def __init__(self):
+
+ #variables
+ self.target_pos = Float32()
+
+ #publishers
+ self.grip_control = rospy.Publisher('/f550_amazing/longgrip_rad', Float32, queue_size=1)
+
+ self.rate = rospy.Rate(20)
+ self.result = simulation_control.msg.long_grippersResult()
+ self.server = actionlib.SimpleActionServer('long_grippers',
+ simulation_control.msg.long_grippersAction,
+ execute_cb=self.execute_cb,
+ auto_start=False)
+ self.server.start()
+
+ # Main function
+ def execute_cb(self, goal):
+ self.grip_control.publish(goal.grip_rad_goal)
+ time.sleep(3)
+ self.result.goal_reached.data = True
+ self.server.set_succeeded(self.result)
+
+
+if __name__ == '__main__':
+ rospy.init_node('long_grippers_server')
+ server = long_grippers_server()
+ rospy.spin()
diff --git a/simulation_control/src/scripts/mavros_state.py b/simulation_control/src/scripts/mavros_state.py
new file mode 100644
index 0000000000000000000000000000000000000000..3a07a6d6fe7cc1659f616933a397119f0ac7d0ff
--- /dev/null
+++ b/simulation_control/src/scripts/mavros_state.py
@@ -0,0 +1,198 @@
+#!/usr/bin/env python
+
+import rospy
+from mavros_msgs.msg import State
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import PoseStamped, TwistStamped
+from mavros_msgs.srv import SetMode, SetModeRequest, SetModeResponse, CommandBool, CommandBoolRequest, CommandBoolResponse, CommandTOL, CommandTOLRequest
+import time
+from tf.transformations import *
+import numpy as np
+
+class mavros_state():
+ def __init__(self):
+ ### subscriber ###
+
+ # state subscriber
+ self._rate_state = rospy.Rate(1)
+ self.current_state = State()
+ rospy.Subscriber('/mavros/state', State, self.current_state_callback)
+
+ # wait until connection with FCU
+ while not rospy.is_shutdown() and not self.current_state.connected:
+ rospy.Rate(20)
+ print 'FCU connection successful'
+
+ #callback function for state
+ def current_state_callback(self, data):
+ self.current_state = data
+
+
+ #setmode for the drone
+ #'OFFBOARD'
+ def set_mode(self, mode):
+ if not self.current_state.connected:
+ print "No FCU connection"
+
+ elif self.current_state.mode == mode:
+ print "Already in " + mode + " mode"
+
+ else:
+
+ # wait for service
+ rospy.wait_for_service("mavros/set_mode")
+
+ # service client
+ set_mode = rospy.ServiceProxy("mavros/set_mode", SetMode)
+
+ # set request object
+ req = SetModeRequest()
+ req.custom_mode = mode
+
+ # zero time
+ t0 = rospy.get_time()
+
+ # check response
+ while not rospy.is_shutdown() and (self.current_state.mode != req.custom_mode):
+ if rospy.get_time() - t0 > 2.0: # check every 5 seconds
+
+ try:
+ # request
+ set_mode.call(req)
+
+ except rospy.ServiceException, e:
+ print "Service did not process request: %s" % str(e)
+
+ t0 = rospy.get_time()
+
+ print "Mode: " + self.current_state.mode + " established"
+
+
+ #Arm the vehicle
+ def arm(self, do_arming):
+
+ if self.current_state.armed and do_arming:
+ print "already armed"
+
+ else:
+ # wait for service
+ rospy.wait_for_service("mavros/cmd/arming")
+
+ # service client
+ set_arm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
+
+ # set request object
+ req = CommandBoolRequest()
+ req.value = do_arming
+
+ # zero time
+ t0 = rospy.get_time()
+
+ # check response
+ if do_arming:
+ while not rospy.is_shutdown() and not self.current_state.armed:
+ if rospy.get_time() - t0 > 2.0: # check every 5 seconds
+
+ try:
+ # request
+ set_arm.call(req)
+
+ except rospy.ServiceException, e:
+ print "Service did not process request: %s" % str(e)
+
+ t0 = rospy.get_time()
+
+ print "armed: ", self.current_state.armed
+
+ else:
+ while not rospy.is_shutdown() and self.current_state.armed:
+ if rospy.get_time() - t0 > 0.5: # check every 5 seconds
+
+ try:
+ # request
+ set_arm.call(req)
+
+ except rospy.ServiceException, e:
+ print "Service did not process request: %s" % str(e)
+
+ t0 = rospy.get_time()
+
+ def land(self, height):
+
+ if not self.current_state.armed:
+
+ print "not armed yet"
+
+ else:
+
+ # wait for service
+ rospy.wait_for_service("mavros/cmd/land")
+
+ # service client
+ set_rq = rospy.ServiceProxy("mavros/cmd/land", CommandTOL)
+
+ # set request object
+ req = CommandTOLRequest()
+ req.yaw = 0.0
+ req.latitude = 0.0
+ req.longitude = 0.0
+ req.altitude = height
+ req.min_pitch = 0.0
+
+ # zero time
+ t0 = rospy.get_time()
+
+ # check response
+ while self.current_state.armed:
+ if rospy.get_time() - t0 > 5.0: # check every 5 seconds
+
+ try:
+ # request
+ set_rq.call(req)
+
+ except rospy.ServiceException, e:
+ print "Service did not process request: %s" % str(e)
+
+ t0 = rospy.get_time()
+
+ print "landed savely"
+
+ def takeoff(self):
+
+ if not self.current_state.armed:
+
+ print "not armed yet"
+
+ else:
+
+ # wait for service
+ rospy.wait_for_service("mavros/cmd/takeoff")
+
+ # service client
+ set_rq = rospy.ServiceProxy("mavros/cmd/takeoff", CommandTOL)
+
+ # set request object
+ req = CommandTOLRequest()
+ req.yaw = 0.0
+ req.latitude = 0.0
+ req.longitude = 0.0
+ req.altitude = 4.0
+ req.min_pitch = 0.0
+
+ # zero time
+ t0 = rospy.get_time()
+
+ # check response
+ while self.current_state.armed:
+ if rospy.get_time() - t0 > 5.0: # check every 5 seconds
+
+ try:
+ # request
+ set_rq.call(req)
+
+ except rospy.ServiceException, e:
+ print "Service did not process request: %s" % str(e)
+
+ t0 = rospy.get_time()
+
+ print "landed savely"
diff --git a/simulation_control/src/scripts/mavros_state.pyc b/simulation_control/src/scripts/mavros_state.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..d3cac770fb3c735ddb158b7d3402b6ca70a69f67
Binary files /dev/null and b/simulation_control/src/scripts/mavros_state.pyc differ
diff --git a/simulation_control/src/scripts/position_control.py b/simulation_control/src/scripts/position_control.py
new file mode 100644
index 0000000000000000000000000000000000000000..1c178d7d74fd97e4c8f9e7a07275be859a0a541a
--- /dev/null
+++ b/simulation_control/src/scripts/position_control.py
@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+from geometry_msgs.msg import PoseStamped, TwistStamped, Point
+import rospy
+import math
+from std_msgs.msg import Float32, Bool, String
+from VelocityController import VelocityController
+import numpy as np
+
+class position_control():
+ def __init__(self):
+ print 'Initialising position control'
+ rospy.init_node('position_control', anonymous=True)
+ rate = rospy.Rate(20)
+
+ rospy.Subscriber('/position_control/set_mode', String, self.set_mode_callback)
+ rospy.Subscriber('/position_control/set_position', PoseStamped, self.set_pose_callback)
+ rospy.Subscriber('/position_control/set_velocity', PoseStamped, self.set_velocity_callback)
+
+ rospy.Subscriber('/position_control/set_x_pid', Point, self.set_x_pid)
+ rospy.Subscriber('/position_control/set_y_pid', Point, self.set_y_pid)
+ rospy.Subscriber('/position_control/set_z_pid', Point, self.set_z_pid)
+
+ # pos
+ self._pose_pub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
+ self._pose_msg = PoseStamped()
+ self._vel_pose_msg = PoseStamped()
+ self._pos_state = "posctr"
+ # vel
+ self._vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel', TwistStamped, queue_size=10)
+ self._vel_msg = TwistStamped()
+ self._vel_state = "velctr"
+
+ self.local_pose = PoseStamped()
+ rospy.Subscriber('/mavros/local_position/pose', PoseStamped, self._local_pose_callback)
+ self.local_velocity = TwistStamped()
+ rospy.Subscriber('/mavros/local_position/velocity', TwistStamped, self._local_velocity_callback)
+ self.dist = rospy.Publisher('/position_control/distance', Bool, queue_size=10)
+
+ self.current_publisher = self._pose_pub
+ self.current_message = self._pose_msg
+ #self._pose_msg.pose.position.z = 3
+ self.set_pose(self._pose_msg)
+ self.des_vel = TwistStamped()
+ self.current_mode = String()
+ self.current_mode.data = 'posctr'
+
+ self.vController = VelocityController()
+ self.vController.set_x_pid(2.8, 0.913921, 0.0, 0.15)
+ self.vController.set_y_pid(2.8, 0.913921, 0.0, 0.15)#2.1, 0.713921, 0.350178
+ self.vController.set_z_pid(1.3, 2.4893, 0.102084, 0.1)
+
+ print 'Init done'
+ while not rospy.is_shutdown():
+ if self.current_mode.data == 'posctr':
+ self._pose_pub.publish(self._pose_msg)
+ elif self.current_mode.data == 'velctr':
+ self.vController.setTarget(self._vel_pose_msg.pose)
+ self.des_vel = self.vController.update(self.local_pose)
+ self._vel_pub.publish(self.des_vel)
+ else:
+ print "No such position mode"
+ self.check_distance()
+ rate.sleep()
+
+ def set_mode_callback(self, data):
+ self.current_mode = data
+
+ def set_vel(self, vel):
+ self._vel_msg.twist.linear.x = vel.twist.linear.x
+ self._vel_msg.twist.linear.y = vel.twist.linear.y
+ self._vel_msg.twist.linear.z = vel.twist.linear.z
+
+ def set_pose(self, pose):
+ self._pose_msg.pose.position.x = pose.pose.position.x
+ self._pose_msg.pose.position.y = pose.pose.position.y
+ self._pose_msg.pose.position.z = pose.pose.position.z
+
+ self._pose_msg.pose.orientation.x = pose.pose.orientation.x
+ self._pose_msg.pose.orientation.y = pose.pose.orientation.y
+ self._pose_msg.pose.orientation.z = pose.pose.orientation.z
+ self._pose_msg.pose.orientation.w = pose.pose.orientation.w
+
+ def set_vel_pose(self, vel_pose):
+ #print(vel_pose.pose)
+ self._vel_pose_msg.pose.position.x = self.local_pose.pose.position.x + vel_pose.pose.position.x
+ self._vel_pose_msg.pose.position.y = self.local_pose.pose.position.y + vel_pose.pose.position.y
+ self._vel_pose_msg.pose.position.z = vel_pose.pose.position.z
+ #print(self._vel_pose_msg.pose)
+ self._vel_pose_msg.pose.orientation.x = vel_pose.pose.orientation.x
+ self._vel_pose_msg.pose.orientation.y = vel_pose.pose.orientation.y
+ self._vel_pose_msg.pose.orientation.z = vel_pose.pose.orientation.z
+ self._vel_pose_msg.pose.orientation.w = vel_pose.pose.orientation.w
+
+ def _local_pose_callback(self, data):
+ self.local_pose = data
+
+ def _local_velocity_callback(self, data):
+ self.local_velocity = data
+
+ def set_pose_callback(self, data):
+ self.set_pose(data)
+
+ def set_velocity_callback(self, data):
+ self.set_vel_pose(data)
+
+ def set_x_pid(self,data):
+ self.vController.set_x_pid(data.x, data.y, data.z)
+
+ def set_y_pid(self,data):
+ self.vController.set_y_pid(data.x, data.y, data.z)
+
+ def set_z_pid(self,data):
+ self.vController.set_z_pid(data.x, data.y, data.z)
+
+ def check_distance(self):
+ if self.current_mode.data == 'posctr':
+ booldist = self.is_at_position(self.local_pose.pose.position, self._pose_msg.pose.position, 0.5)
+ boolvel = self.hover_velocity()
+ self.dist.publish(booldist and boolvel)
+ elif self.current_mode.data == 'velctr':
+ vel_pose_tot = PoseStamped()
+ vel_pose_tot.pose.position.x = self._vel_pose_msg.pose.position.x
+ vel_pose_tot.pose.position.y = self._vel_pose_msg.pose.position.y
+ vel_pose_tot.pose.position.z = self._vel_pose_msg.pose.position.z
+ #print("target vel_pos: {}".format(vel_pose_tot))
+ booldist = self.is_at_position(self.local_pose.pose.position, vel_pose_tot.pose.position, 0.2)
+ boolvel = self.hover_velocity()
+ self.dist.publish(booldist and boolvel)
+
+ def is_at_position(self,p_current, p_desired, offset):
+ des_pos = np.array((p_desired.x,
+ p_desired.y,
+ p_desired.z))
+ cur_pos = np.array((p_current.x,
+ p_current.y,
+ p_current.z))
+ distance = np.linalg.norm(des_pos - cur_pos)
+ return distance < offset
+
+ def hover_velocity(self):
+ return self.local_velocity.twist.linear.x < 0.2 and self.local_velocity.twist.linear.y < 0.2 and self.local_velocity.twist.linear.z < 0.2
+
+if __name__ == '__main__':
+ try:
+
+ position_control()
+
+ except rospy.ROSInterruptException:
+ pass
+
diff --git a/simulation_control/src/scripts/short_grippers_server.py b/simulation_control/src/scripts/short_grippers_server.py
new file mode 100644
index 0000000000000000000000000000000000000000..e7bf2dfaa3f99b014996a7fb04ed3d01ce501653
--- /dev/null
+++ b/simulation_control/src/scripts/short_grippers_server.py
@@ -0,0 +1,39 @@
+#!/usr/bin/env python
+
+import roslib
+import rospy
+import actionlib
+import simulation_control.msg
+import time
+
+from std_msgs.msg import Float32
+
+class short_grippers_server():
+ def __init__(self):
+
+ #variables
+ self.target_pos = Float32()
+
+ #publishers
+ self.grip_control = rospy.Publisher('/f550_amazing/grip_rad', Float32, queue_size=1)
+
+ self.rate = rospy.Rate(20)
+ self.result = simulation_control.msg.short_grippersResult()
+ self.server = actionlib.SimpleActionServer('short_grippers',
+ simulation_control.msg.short_grippersAction,
+ execute_cb=self.execute_cb,
+ auto_start=False)
+ self.server.start()
+
+ # Main function
+ def execute_cb(self, goal):
+ self.grip_control.publish(goal.grip_rad_goal)
+ time.sleep(3)
+ self.result.goal_reached.data = True
+ self.server.set_succeeded(self.result)
+
+
+if __name__ == '__main__':
+ rospy.init_node('short_grippers_server')
+ server = short_grippers_server()
+ rospy.spin()
diff --git a/simulation_control/src/worlds/timpa_airfield.world b/simulation_control/src/worlds/timpa_airfield.world
new file mode 100644
index 0000000000000000000000000000000000000000..b6622705706d9b19fd9f2471e9f4cd8ef04aad6c
--- /dev/null
+++ b/simulation_control/src/worlds/timpa_airfield.world
@@ -0,0 +1,64 @@
+<sdf version='1.6'>
+ <world name='default'>
+
+ <gui>
+ <camera name="user_camera">
+ <pose>-197.304 -5.387 72.475 0 0.55 0</pose>
+ </camera>
+ </gui>
+
+ <include>
+ <uri>model://sun</uri>
+ </include>
+
+ <include>
+ <pose frame=''>0 0 0 0 0 1.5708</pose>
+ <uri>model://landing_strip</uri>
+ </include>
+
+ <include>
+ <pose frame=''>-105 0 0.2 0 0 0.523599</pose>
+ <uri>model://f550_amazing_dead_blue</uri>
+ </include>
+
+ <physics name='default_physics' default='1' type='ode'> # default = name=default_physics, default=0, type=ode
+ <max_step_size>0.002</max_step_size> # default = 0.001
+ <real_time_factor>1.0</real_time_factor> # default = 1
+ <real_time_update_rate>500</real_time_update_rate> # default = 1000
+ <max_contacts>10</max_contacts> # default = 20
+ <ode> # default = ode - alternative = bullet, simbody, dart
+ <solver>
+ <type>quick</type> # default = quick - alternative = world (slower)
+ <min_step_size>0.0001</min_step_size> # default = 0.0001
+ <iters>10</iters> # default = 50
+ <sor>1.3</sor> # default = 1.3
+ <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling> # default = 0
+ <friction_model>pyramid_model</friction_model> # default = pyramid_model
+ </solver>
+ <constraints>
+ <cfm>0</cfm> # default = 0
+ <erp>0.2</erp> # default = 0.2
+ <contact_max_correcting_vel>100</contact_max_correcting_vel> # default = 100
+ <contact_surface_layer>0.001</contact_surface_layer> # default = 0.001
+ </constraints>
+ </ode>
+ </physics>
+
+ <gravity>0 0 -9.8066</gravity> # default = 0 0 -9.8
+ <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field> # default = 6e-06 2.3e-05 -4.2e-05
+ <atmosphere type='adiabatic'/> # default = adiabatic
+
+ <scene>
+ <shadows>0</shadows>
+ </scene>
+
+ <spherical_coordinates>
+ <surface_model>EARTH_WGS84</surface_model>
+ <latitude_deg>0</latitude_deg>
+ <longitude_deg>0</longitude_deg>
+ <elevation>0</elevation>
+ <heading_deg>0</heading_deg>
+ </spherical_coordinates>
+
+ </world>
+</sdf>