init commit

CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
+
+
+project(ardupilot_sitl_gazebo)
+
+
+#######################
+## Find Dependencies ##
+#######################
+
+find_package(gazebo REQUIRED)
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+if("${GAZEBO_VERSION}" VERSION_LESS "7.0")
+    message(FATAL_ERROR "You need at least Gazebo 7.0. Your version: ${GAZEBO_VERSION}")
+else()
+    message("Gazebo version: ${GAZEBO_VERSION}")
+endif()
+
+###########
+## Build ##
+###########
+
+include_directories(
+        ${PROJECT_SOURCE_DIR}
+        include
+        ${GAZEBO_INCLUDE_DIRS}
+        )
+
+link_libraries(
+        ${CMAKE_CURRENT_BINARY_DIR}
+        ${GAZEBO_LIBRARIES}
+        )
+
+link_directories(
+        ${GAZEBO_LIBRARY_DIRS}
+        )
+
+set (plugins_single_header
+        ArduPilotPlugin
+        )
+#add_library(ArduCopterIRLockPlugin SHARED src/ArduCopterIRLockPlugin.cc)
+#target_link_libraries(ArduCopterIRLockPlugin ${GAZEBO_LIBRARIES})
+
+add_library(ArduPilotPlugin SHARED src/ArduPilotPlugin.cc)
+target_link_libraries(ArduPilotPlugin ${GAZEBO_LIBRARIES})
+
+#install(TARGETS ArduCopterIRLockPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})
+install(TARGETS ArduPilotPlugin DESTINATION ${GAZEBO_PLUGIN_PATH})

include/ArduCopterIRLockPlugin.hh

@@ -0,0 +1,66 @@
+/*
+ * Copyright (C) 2016 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#ifndef _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_
+#define _GAZEBO_ARDUCOPTERIRLOCK_PLUGIN_HH_
+
+#include <string>
+#include <memory>
+
+#include "gazebo/common/Plugin.hh"
+#include "gazebo/util/system.hh"
+
+namespace gazebo
+{
+  // Forward declare private class.
+  class ArduCopterIRLockPluginPrivate;
+
+  /// \brief A camera sensor plugin for fiducial detection
+  class GAZEBO_VISIBLE ArduCopterIRLockPlugin : public SensorPlugin
+  {
+    /// \brief Constructor
+    public: ArduCopterIRLockPlugin();
+
+    /// \brief Destructor
+    public: virtual ~ArduCopterIRLockPlugin();
+
+    // Documentation Inherited.
+    public: void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf);
+
+    /// \brief Callback when a new camera frame is available
+    /// \param[in] _image image data
+    /// \param[in] _width image width
+    /// \param[in] _height image height
+    /// \param[in] _depth image depth
+    /// \param[in] _format image format
+    public: virtual void OnNewFrame(const unsigned char *_image,
+        unsigned int _width, unsigned int _height, unsigned int _depth,
+        const std::string &_format);
+
+    /// \brief Publish the result
+    /// \param[in] _name Name of fiducial
+    /// \param[in] _x x position in image
+    /// \param[in] _y y position in image
+    public: virtual void Publish(const std::string &_fiducial, unsigned int _x,
+        unsigned int _y);
+
+    /// \internal
+    /// \brief Pointer to private data.
+    private: std::unique_ptr<ArduCopterIRLockPluginPrivate> dataPtr;
+  };
+}
+#endif

include/ArduPilotPlugin.hh

@@ -0,0 +1,96 @@
+/*
+ * Copyright (C) 2016 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+#ifndef GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_
+#define GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_
+
+#include <sdf/sdf.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/physics/physics.hh>
+
+namespace gazebo
+{
+  // Forward declare private data class
+  class ArduPilotSocketPrivate;
+  class ArduPilotPluginPrivate;
+
+  /// \brief Interface ArduPilot from ardupilot stack
+  /// modeled after SITL/SIM_*
+  ///
+  /// The plugin requires the following parameters:
+  /// <control>             control description block
+  ///    <!-- inputs from Ardupilot -->
+  ///    channel            attribute, ardupilot control channel
+  ///    multiplier         command multiplier
+  ///    <!-- output to Gazebo -->
+  ///    type               type of control, VELOCITY, POSITION or EFFORT
+  ///    <p_gain>           velocity pid p gain
+  ///    <i_gain>           velocity pid i gain
+  ///    <d_gain>           velocity pid d gain
+  ///    <i_max>            velocity pid max integral correction
+  ///    <i_min>            velocity pid min integral correction
+  ///    <cmd_max>          velocity pid max command torque
+  ///    <cmd_min>          velocity pid min command torque
+  ///    <jointName>        motor joint, torque applied here
+  ///    <turningDirection> rotor turning direction, 'cw' or 'ccw'
+  ///    frequencyCutoff    filter incoming joint state
+  ///    samplingRate       sampling rate for filtering incoming joint state
+  ///    <rotorVelocitySlowdownSim> for rotor aliasing problem, experimental
+  /// <imuName>     scoped name for the imu sensor
+  /// <connectionTimeoutMaxCount> timeout before giving up on
+  ///                             controller synchronization
+  class GAZEBO_VISIBLE ArduPilotPlugin : public ModelPlugin
+  {
+    /// \brief Constructor.
+    public: ArduPilotPlugin();
+
+    /// \brief Destructor.
+    public: ~ArduPilotPlugin();
+
+    // Documentation Inherited.
+    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
+
+    /// \brief Update the control surfaces controllers.
+    /// \param[in] _info Update information provided by the server.
+    private: void OnUpdate();
+
+    /// \brief Update PID Joint controllers.
+    /// \param[in] _dt time step size since last update.
+    private: void ApplyMotorForces(const double _dt);
+
+    /// \brief Reset PID Joint controllers.
+    private: void ResetPIDs();
+
+    /// \brief Receive motor commands from ArduPilot
+    private: void ReceiveMotorCommand();
+
+    /// \brief Send state to ArduPilot
+    private: void SendState() const;
+
+    /// \brief Init ardupilot socket
+    private: bool InitArduPilotSockets(sdf::ElementPtr _sdf) const;
+
+    /// \brief Private data pointer.
+    private: std::unique_ptr<ArduPilotPluginPrivate> dataPtr;
+
+    /// \brief transform from model orientation to x-forward and z-up
+    private: ignition::math::Pose3d modelXYZToAirplaneXForwardZDown;
+
+    /// \brief transform from world frame to NED frame
+    private: ignition::math::Pose3d gazeboXYZToNED;
+  };
+}
+#endif