amov-lab / Prometheus安装编译

官方教程：
https://github.com/amov-lab/Prometheus_User_Guide/blob/master/%E5%BC%80%E5%8F%91%E8%80%85%E6%89%8B%E5%86%8C%20-%20%E5%AE%89%E8%A3%85%E5%8F%8A%E7%BC%96%E8%AF%91.md

本人采用ubuntu16.04,对应ros版本kinetic。

1.建图模块依赖项安装

sudo apt-get install ros-kinetic-rtabmap*
sudo apt-get install ros-kinetic-octomap-*
sudo apt-get install ros-kinetic-cartographer*

2.git下载

使用git前可以先加速，参考
https://blog.csdn.net/qq_44621510/article/details/95251993
https://www.ipaddress.com/

sudo vim /etc/hosts

140.82.113.3 github.com
199.232.69.194 github.global.ssl.fastly.net
140.82.114.9 codeload.github.com
#热点
140.82.113.3 github.com
199.232.69.194 github.global.ssl.fastly.net
140.82.113.10 codeload.github.com

sudo /etc/init.d/networking restart

2.1规划模块依赖项安装

安装非线性优化工具箱 NLopt，安装方法如下

git clone git://github.com/stevengj/nlopt  
cd nlopt  
mkdir build  
cd build  
cmake ..  
make  
sudo make install  

2.1源码

使用crtl+alt+T打开一个新的终端

git clone https://github.com/amov-lab/Prometheus

环境变量配置，打开一个新终端，输入gedit .bashrc并回车，在打开的bashrc.txt文件中添加

source /home/ycx/Prometheus/devel/setup.bash

source ~/.bashrc
cd Prometheus
sudo chmod 777 ./complie_all.sh //(第一次运行才需要执行此赋权命令)
./complie_all.sh

错误一：

CMake Error at /opt/ros/kinetic/share/catkin/cmake/catkinConfig.cmake:83 (find_package):
  Could not find a package configuration file provided by "mavros_msgs" with
  any of the following names:
    mavros_msgsConfig.cmake
    mavros_msgs-config.cmake

参考
https://blog.csdn.net/spurmoon/article/details/89281932

 sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras

其他错误，我就贴一下，因该不影响后面结果（可略）

Looking for pthread_create in pthread - found-- Configuring done
CMake Warning (dev) at CMakeLists.txt:306 (add_dependencies):
  Policy CMP0046 is not set: Error on non-existent dependency in
  add_dependencies.  Run "cmake --help-policy CMP0046" for policy details.
  Use the cmake_policy command to set the policy and suppress this warning.

  The dependency target "darknet_ros_msgs_generate_messages_cpp" of target
  "darknet_ros_lib" does not exist.
This warning is for project developers.  Use -Wno-dev to suppress it.


  system
CMake Warning at /home/ycx/Prometheus/build/mission/cmake/prometheus_mission-genmsg.cmake:3 (message):
  Invoking generate_messages() without having added any message or service
  file before.

  You should either add add_message_files() and/or add_service_files() calls
  or remove the invocation of generate_messages().
Call Stack (most recent call first):
  /opt/ros/kinetic/share/genmsg/cmake/genmsg-extras.cmake:307 (include)
  CMakeLists.txt:35 (generate_messages)



CMake Warning (dev) at CMakeLists.txt:34 (find_package):
  Policy CMP0074 is not set: find_package uses <PackageName>_ROOT variables.
  Run "cmake --help-policy CMP0074" for policy details.  Use the cmake_policy
  command to set the policy and suppress this warning.

  CMake variable PCL_ROOT is set to:

    /usr/local

  For compatibility, CMake is ignoring the variable.
This warning is for project developers.  Use -Wno-dev to suppress it.


-- Checking for module 'openni-dev'
--   No package 'openni-dev' found
-- Checking for module 'openni2-dev'
--   No package 'openni2-dev' found

CMake Warning at FastPlanner/plan_env/ThirdParty/sdf_tools/CMakeLists.txt:13 (find_package):
  By not providing "Findmoveit_msgs.cmake" in CMAKE_MODULE_PATH this project
  has asked CMake to find a package configuration file provided by
  "moveit_msgs", but CMake did not find one.

  Could not find a package configuration file provided by "moveit_msgs" with
  any of the following names:

    moveit_msgsConfig.cmake
    moveit_msgs-config.cmake

  Add the installation prefix of "moveit_msgs" to CMAKE_PREFIX_PATH or set
  "moveit_msgs_DIR" to a directory containing one of the above files.  If
  "moveit_msgs" provides a separate development package or SDK, be sure it
  has been installed.


CMake Warning at FastPlanner/plan_env/ThirdParty/sdf_tools/CMakeLists.txt:14 (find_package):
  By not providing "Findmoveit_core.cmake" in CMAKE_MODULE_PATH this project
  has asked CMake to find a package configuration file provided by
  "moveit_core", but CMake did not find one.

  Could not find a package configuration file provided by "moveit_core" with
  any of the following names:

    moveit_coreConfig.cmake
    moveit_core-config.cmake

  Add the installation prefix of "moveit_core" to CMAKE_PREFIX_PATH or set
  "moveit_core_DIR" to a directory containing one of the above files.  If
  "moveit_core" provides a separate development package or SDK, be sure it
  has been installed.


CMake Warning at FastPlanner/plan_env/ThirdParty/sdf_tools/CMakeLists.txt:15 (find_package):
  By not providing "Findmoveit_ros_planning.cmake" in CMAKE_MODULE_PATH this
  project has asked CMake to find a package configuration file provided by
  "moveit_ros_planning", but CMake did not find one.

  Could not find a package configuration file provided by
  "moveit_ros_planning" with any of the following names:

    moveit_ros_planningConfig.cmake
    moveit_ros_planning-config.cmake

  Add the installation prefix of "moveit_ros_planning" to CMAKE_PREFIX_PATH
  or set "moveit_ros_planning_DIR" to a directory containing one of the above
  files.  If "moveit_ros_planning" provides a separate development package or
  SDK, be sure it has been installed.



CMake Warning at global_planner/CMakeLists.txt:49 (add_executable):
  Cannot generate a safe runtime search path for target global_planner_main
  because files in some directories may conflict with libraries in implicit
  directories:

    runtime library [libpcl_common.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_kdtree.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_octree.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_search.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_io.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_sample_consensus.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_filters.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_features.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_segmentation.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_surface.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_registration.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_recognition.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_keypoints.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_visualization.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_people.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_outofcore.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib
    runtime library [libpcl_tracking.so.1.7] in /usr/lib/x86_64-linux-gnu may be hidden by files in:
      /usr/local/lib

  Some of these libraries may not be found correctly.

3.仿真

官方教程：
https://github.com/amov-lab/Prometheus_User_Guide/blob/master/%E5%BC%80%E5%8F%91%E8%80%85%E6%89%8B%E5%86%8C%20-%20Gazebo%E4%BB%BF%E7%9C%9F%E7%8E%AF%E5%A2%83%E5%AE%89%E8%A3%85%E5%8F%8A%E6%B5%8B%E8%AF%95.md

px4

官方的意思应该是直接用ubuntu.sh脚本进行安装,而且最好把Firmware_v110（PX4固件代码）放在Prometheus目录下，这样就在一个工作空间。

sudo apt install gcc-arm-none-eabi
git clone https://github.com/amov-lab/Firmware_v110

//or
git clone https://gitee.com/ckwu1201/Firmware_v110.git


sudo usermod -a -G dialout $USER
sudo apt-get remove modemmanager -y 

注销并重新登录

cd /home/ycx/Prometheus/Firmware_v110/Tools/setup
./ubuntu.sh

这样px4应该好了，可以重启电脑。
继续

cd Firmware_v110
git submodule update --init --recursive
//中间可能会断开，在试一次就好了
git submodule update --init --recursive
make px4_sitl gazebo

我这里
fatal: 无法克隆 ‘https://github.com/PX4/jMAVSim.git’ 到子模组路径 ‘Tools/jMAVSim’，我直接去github下了改好名字，放到指定目录。
或者

cd Tools
git clone https://gitee.com/bjtyxc/jMAVSim.git
cd ..
git submodule update --init --recursive
make px4_sitl gazebo

不行可能px4有问题，参考https://blog.csdn.net/oqqenvy12/article/details/79266838
sudo usermod -a -G dialout $USER
sudo apt-get remove modemmanager -y
注销并重新登录
保存如下脚本ubuntu_sim_nuttx.sh:

#!/bin/bash

## Bash script for setting up a PX4 development environment for Pixhawk/NuttX targets on Ubuntu LTS (16.04).
## It can be used for installing simulators and the NuttX toolchain.
##
## Installs:
## - Common dependencies libraries, tools, and Gazebo8 simulator as defined in `ubuntu_sim.sh`
## - NuttX toolchain (i.e. gcc compiler)

echo "Downloading dependent script 'ubuntu_sim.sh'"
# Source the ubuntu_sim.sh script directly from github
ubuntu_sim=$(wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim.sh -O -)
wget_return_code=$?
# If there was an error downloading the dependent script, we must warn the user and exit at this point.
if [[ $wget_return_code -ne 0 ]]; then echo "Error downloading 'ubuntu_sim.sh'. Sorry but I cannot proceed further :("; exit 1; fi
# Otherwise source the downloaded script.
. <(echo "${ubuntu_sim}")

# NuttX
sudo apt-get install python-serial openocd \
    flex bison libncurses5-dev autoconf texinfo \
    libftdi-dev libtool zlib1g-dev -y

# Clean up old GCC
sudo apt-get remove gcc-arm-none-eabi gdb-arm-none-eabi binutils-arm-none-eabi gcc-arm-embedded -y
sudo add-apt-repository --remove ppa:team-gcc-arm-embedded/ppa -y


# GNU Arm Embedded Toolchain: 7-2017-q4-major December 18, 2017
gcc_dir=$HOME/gcc-arm-none-eabi-7-2017-q4-major
echo "Installing GCC to: $gcc_dir"
if [ -d "$gcc_dir" ]
then
    echo " GCC already installed."
else
    pushd .
    cd ~    
    wget https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/7-2017q4/gcc-arm-none-eabi-7-2017-q4-major-linux.tar.bz2
    tar -jxf gcc-arm-none-eabi-7-2017-q4-major-linux.tar.bz2
    exportline="export PATH=$HOME/gcc-arm-none-eabi-7-2017-q4-major/bin:\$PATH"
    if grep -Fxq "$exportline" ~/.profile; then echo " GCC path already set." ; else echo $exportline >> ~/.profile; fi
    . ~/.profile
    popd
fi


#Reboot the computer (required before building)
echo RESTART YOUR COMPUTER to complete installation of PX4 development toolchain

使脚本生效，执行指令
source ubuntu_sim_nuttx.sh
重启电脑
在~/src目录下已经下载好了最新的PX4固件

环境变量配置

这里顺序可能也有要求，按官网来
sudo gedit ~/.bashrc

source /opt/ros/kinetic/setup.bash
source /home/ycx/Prometheus/devel/setup.bash
export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:/home/ycx/Prometheus/devel/lib
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/home/ycx/Prometheus/Simulator/gazebo_simulator/models
source /home/ycx/Prometheus/Firmware_v110/Tools/setup_gazebo.bash /home/ycx/Prometheus/Firmware_v110 /home/ycx/Prometheus/Firmware_v110/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/home/ycx/Prometheus/Firmware_v110
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/home/ycx/Prometheus/Firmware_v110/Tools/sitl_gazebo

测试

sudo apt-get install ros-kinetic-velodyne-gazebo-plugins
sudo apt-get install ros-kinetic-octomap-rviz-plugins

cd Prometheus
./complie_gazebo_simulator.sh 
roslaunch prometheus_gazebo sitl.launch

新终端

rosrun prometheus_control terminal_control//终端控制（全自主的offboard控制）

没反应，看终端有报错，
INFO [simulator] Waiting for simulator to connect on TCP port 4560 [FATAL] [1594730537.149578210]: UAS: GeographicLib exception: File not readable /usr/share/GeographicLib/geoids/egm96-5.pgm | Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!

sudo find / -name install_geographiclib_datasets.sh
sudo /opt/ros/kinetic/lib/mavros/install_geographiclib_datasets.sh

ok，首先输入999解锁并切换至offboard模式，然后输入1起飞，然后再输入其他指令（悬停、降落、机体系移动、惯性系移动、轨迹追踪等等）

QGC地面站安装

官网是3.5.6版本
https://blog.csdn.net/weixin_44747240/article/details/105054947
https://github.com/mavlink/qgroundcontrol/releases/tag/v3.5.6
下载这两个包，AppImage可以不下

下载完成后解压tar.bz2
sudo apt-get install espeak libespeak-dev libudev-dev libsdl1.2-dev
进入解压的QGC文件夹内，运行
./qgroundcontrol-start.sh

安装APPImage，可以很简单的从桌面图标打开QGC
chmod +x ./QGroundControl.AppImage
./QGroundControl.AppImage

测试
roslaunch prometheus_gazebo sitl.launch
打开QGC，就可以由QGC控制px4

最后一个问题

这个问题因为时间有点了，当时也没记录不知道是不是这里的错误，具体是未定义的函数成员Name和SimTime，

gedit /usr/include/gazebo-7/gazebo/physics/World.hh

在/usr/include/gazebo-7/gazebo/physics/World.hh里添加

public: std::string Name() const;
 public: common::Time SimTime() const;

文件全部：

/*
 * Copyright (C) 2012-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_WORLD_HH_
#define _GAZEBO_WORLD_HH_

#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
  #include <Winsock2.h>
#endif

#include <vector>
#include <list>
#include <set>
#include <deque>
#include <string>
#include <boost/thread.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/shared_ptr.hpp>

#include <sdf/sdf.hh>

#include "gazebo/transport/TransportTypes.hh"

#include "gazebo/msgs/msgs.hh"

#include "gazebo/common/CommonTypes.hh"
#include "gazebo/common/UpdateInfo.hh"
#include "gazebo/common/Event.hh"

#include "gazebo/physics/Base.hh"
#include "gazebo/physics/PhysicsTypes.hh"
#include "gazebo/physics/WorldState.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace physics
  {
    /// Forward declare private data class.
    class WorldPrivate;

    /// \addtogroup gazebo_physics
    /// \{

    /// \class World World.hh physics/physics.hh
    /// \brief The world provides access to all other object within a simulated
    /// environment.
    ///
    /// The World is the container for all models and their components
    /// (links, joints, sensors, plugins, etc), and WorldPlugin instances.
    /// Many core function are also handled in the World, including physics
    /// update, model updates, and message processing.
    class GZ_PHYSICS_VISIBLE World :
      public boost::enable_shared_from_this<World>
    {
      /// \brief Constructor.
      /// Constructor for the World. Must specify a unique name.
      /// \param[in] _name Name of the world.
      public: explicit World(const std::string &_name = "");

      /// \brief Destructor.
      public: ~World();

public: std::string Name() const;
 public: common::Time SimTime() const;

      /// \brief Load the world using SDF parameters.
      /// Load a world from and SDF pointer.
      /// \param[in] _sdf SDF parameters.
      public: void Load(sdf::ElementPtr _sdf);

      /// \brief Save a world to a file.
      /// Save the current world and its state to a file.
      /// \param[in] _filename Name of the file to save into.
      public: void Save(const std::string &_filename);

      /// \brief Initialize the world.
      /// This is called after Load.
      public: void Init();

      /// \brief Run the world in a thread.
      /// Run the update loop.
      /// \param[in] _iterations Run for this many iterations, then stop.
      /// A value of zero disables run stop.
      public: void Run(unsigned int _iterations = 0);

      /// \brief Return the running state of the world.
      /// \return True if the world is running.
      public: bool GetRunning() const;

      /// \brief Stop the world.
      /// Stop the update loop.
      public: void Stop();

      /// \brief Finalize the world.
      /// Call this function to tear-down the world.
      public: void Fini();

      /// \brief Remove all entities from the world.
      /// This function has delayed effect. Models are cleared at the end
      /// of the current update iteration.
      public: void Clear();

      /// \brief Get the name of the world.
      /// \return The name of the world.
      public: std::string GetName() const;

      /// \brief Return the physics engine.
      /// Get a pointer to the physics engine used by the world.
      /// \return Pointer to the physics engine.
      public: PhysicsEnginePtr GetPhysicsEngine() const;

      /// \brief Return the preset manager.
      /// \return Pointer to the preset manager.
      public: PresetManagerPtr GetPresetManager() const;

      /// \brief Return the spherical coordinates converter.
      /// \return Pointer to the spherical coordinates converter.
      public: common::SphericalCoordinatesPtr GetSphericalCoordinates() const;

      /// \brief Return the gravity vector.
      /// \return The gravity vector.
      public: ignition::math::Vector3d Gravity() const;

      /// \brief Set the gravity vector.
      /// \param[in] _gravity New gravity vector.
      public: void SetGravity(const ignition::math::Vector3d &_gravity);

      /// \brief Set the gravity sdf value.
      /// \param[in] _gravity New gravity vector.
      public: void SetGravitySDF(const ignition::math::Vector3d &_gravity);

      /// \brief Return the magnetic field vector.
      /// \return The magnetic field vector.
      public: virtual ignition::math::Vector3d MagneticField() const;

      /// \brief Set the magnetic field vector.
      /// \param[in] _mag New magnetic field vector.
      public: void SetMagneticField(const ignition::math::Vector3d &_mag);

      /// \brief Get the number of models.
      /// \return The number of models in the World.
      public: unsigned int GetModelCount() const;

      /// \brief Get a model based on an index.
      /// Get a Model using an index, where index must be greater than zero
      /// and less than World::GetModelCount()
      /// \param[in] _index The index of the model [0..GetModelCount)
      /// \return A pointer to the Model. NULL if _index is invalid.
      public: ModelPtr GetModel(unsigned int _index) const;

      /// \brief Get a list of all the models.
      /// \return A list of all the Models in the world.
      public: Model_V GetModels() const;

      /// \brief Get a list of all the lights.
      /// \return A list of all the Lights in the world.
      public: Light_V Lights() const;

      /// \brief Reset with options.
      /// The _type parameter specifies which type of eneities to reset. See
      /// Base::EntityType.
      /// \param[in] _type The type of reset.
      public: void ResetEntities(Base::EntityType _type = Base::BASE);

      /// \brief Reset simulation time back to zero.
      public: void ResetTime();

      /// \brief Reset time and model poses, configurations in simulation.
      public: void Reset();

      /// \brief Print Entity tree.
      /// Prints alls the entities to stdout.
      public: void PrintEntityTree();

      /// \brief Get the world simulation time, note if you want the PC
      /// wall clock call common::Time::GetWallTime.
      /// \return The current simulation time
      public: common::Time GetSimTime() const;

      /// \brief Set the sim time.
      /// \param[in] _t The new simulation time
      public: void SetSimTime(const common::Time &_t);

      /// \brief Get the amount of time simulation has been paused.
      /// \return The pause time.
      public: common::Time GetPauseTime() const;

      /// \brief Get the wall time simulation was started..
      /// \return The start time.
      public: common::Time GetStartTime() const;

      /// \brief Get the real time (elapsed time).
      /// \return The real time.
      public: common::Time GetRealTime() const;

      /// \brief Returns the state of the simulation true if paused.
      /// \return True if paused.
      public: bool IsPaused() const;

      /// \brief Set whether the simulation is paused.
      /// \param[in] _p True pauses the simulation. False runs the simulation.
      public: void SetPaused(bool _p);

      /// \brief Get an element by name.
      /// Searches the list of entities, and return a pointer to the model
      /// with a matching _name.
      /// \param[in] _name The name of the Model to find.
      /// \return A pointer to the entity, or NULL if no entity was found.
      public: BasePtr GetByName(const std::string &_name);

      /// \brief Get a model by name.
      /// This function is the same as GetByName, but limits the search to
      /// only models.
      /// \param[in] _name The name of the Model to find.
      /// \return A pointer to the Model, or NULL if no model was found.
      public: ModelPtr GetModel(const std::string &_name);

      /// \brief Get a light by name.
      /// This function is the same as GetByName, but limits the search to
      /// only lights.
      /// \param[in] _name The name of the Light to find.
      /// \return A pointer to the Light, or NULL if no light was found.
      public: LightPtr Light(const std::string &_name);

      /// \brief Get a pointer to an Entity based on a name.
      /// This function is the same as GetByName, but limits the search to
      /// only Entities.
      /// \param[in] _name The name of the Entity to find.
      /// \return A pointer to the Entity, or NULL if no Entity was found.
      public: EntityPtr GetEntity(const std::string &_name);

      /// \brief Get the nearest model below and not encapsulating a point.
      /// Only objects below the start point can be returned. Any object
      /// that encapsulates the start point can not be returned from this
      /// function.
      /// This function makes use of World::GetEntityBelowPoint.
      /// \param[in] _pt The 3D point to search below.
      /// \return A pointer to nearest Model, NULL if none is found.
      public: ModelPtr GetModelBelowPoint(const math::Vector3 &_pt);

      /// \brief Get the nearest entity below a point.
      /// Projects a Ray down (-Z axis) starting at the given point. The
      /// first entity hit by the Ray is returned.
      /// \param[in] _pt The 3D point to search below
      /// \return A pointer to nearest Entity, NULL if none is found.
      public: EntityPtr GetEntityBelowPoint(const math::Vector3 &_pt);

      /// \brief Set the current world state.
      /// \param _state The state to set the World to.
      public: void SetState(const WorldState &_state);

      /// \brief Insert a model from an SDF file.
      /// Spawns a model into the world base on and SDF file.
      /// \param[in] _sdfFilename The name of the SDF file (including path).
      public: void InsertModelFile(const std::string &_sdfFilename);

      /// \brief Insert a model from an SDF string.
      /// Spawns a model into the world base on and SDF string.
      /// \param[in] _sdfString A string containing valid SDF markup.
      public: void InsertModelString(const std::string &_sdfString);

      /// \brief Insert a model using SDF.
      /// Spawns a model into the world base on and SDF object.
      /// \param[in] _sdf A reference to an SDF object.
      public: void InsertModelSDF(const sdf::SDF &_sdf);

      /// \brief Return a version of the name with "<world_name>::" removed
      /// \param[in] _name Usually the name of an entity.
      /// \return The stripped world name.
      public: std::string StripWorldName(const std::string &_name) const;

      /// \brief Enable all links in all the models.
      /// Enable is a physics concept. Enabling means that the physics
      /// engine should update an entity.
      public: void EnableAllModels();

      /// \brief Disable all links in all the models.
      /// Disable is a physics concept. Disabling means that the physics
      /// engine should not update an entity.
      public: void DisableAllModels();

      /// \brief Step the world forward in time.
      /// \param[in] _steps The number of steps the World should take.
      public: void Step(unsigned int _steps);

      /// \brief Load a plugin
      /// \param[in] _filename The filename of the plugin.
      /// \param[in] _name A unique name for the plugin.
      /// \param[in] _sdf The SDF to pass into the plugin.
      public: void LoadPlugin(const std::string &_filename,
                              const std::string &_name,
                              sdf::ElementPtr _sdf);

      /// \brief Remove a running plugin.
      /// \param[in] _name The unique name of the plugin to remove.
      public: void RemovePlugin(const std::string &_name);

      /// \brief Get the set world pose mutex.
      /// \return Pointer to the mutex.
      public: boost::mutex *GetSetWorldPoseMutex() const;

      /// \brief check if physics engine is enabled/disabled.
      /// \param True if the physics engine is enabled.
      public: bool GetEnablePhysicsEngine();

      /// \brief enable/disable physics engine during World::Update.
      /// \param[in] _enable True to enable the physics engine.
      public: void EnablePhysicsEngine(bool _enable);

      /// \brief Update the state SDF value from the current state.
      public: void UpdateStateSDF();

      /// \brief Return true if the world has been loaded.
      /// \return True if World::Load has completed.
      public: bool IsLoaded() const;

      /// \brief Remove all entities from the world. Implementation of
      /// World::Clear
      public: void ClearModels();

      /// \brief Publish pose updates for a model.
      /// This list of models to publish is processed and cleared once every
      /// iteration.
      /// \param[in] _model Pointer to the model to publish.
      public: void PublishModelPose(physics::ModelPtr _model);

      /// \brief Publish scale updates for a model.
      /// This list of models to publish is processed and cleared once every
      /// iteration.
      /// \param[in] _model Pointer to the model to publish.
      public: void PublishModelScale(physics::ModelPtr _model);

      /// \brief Publish pose updates for a light.
      /// Adds light to a list of lights to publish, which is processed and
      /// cleared once every iteration.
      /// \param[in] _light Pointer to the light to publish.
      public: void PublishLightPose(const physics::LightPtr _light);

      /// \brief Get the total number of iterations.
      /// \return Number of iterations that simulation has taken.
      public: uint32_t GetIterations() const;

      /// \brief Get the current scene in message form.
      /// \return The scene state as a protobuf message.
      public: msgs::Scene GetSceneMsg() const;

      /// \brief Run the world. This call blocks.
      /// Run the update loop.
      /// \param[in] _iterations Run for this many iterations, then stop.
      /// A value of zero disables run stop.
      public: void RunBlocking(unsigned int _iterations = 0);

      /// \brief Remove a model. This function will block until
      /// the physics engine is not locked. The duration of the block
      /// is less than the time to complete a simulation iteration.
      /// \param[in] _model Pointer to a model to remove.
      public: void RemoveModel(ModelPtr _model);

      /// \brief Remove a model by name. This function will block until
      /// the physics engine is not locked. The duration of the block
      /// is less than the time to complete a simulation iteration.
      /// \param[in] _name Name of the model to remove.
      public: void RemoveModel(const std::string &_name);

      /// \brief Reset the velocity, acceleration, force and torque of
      /// all child models.
      public: void ResetPhysicsStates();

      /// \internal
      /// \brief Inform the World that an Entity has moved. The Entity
      /// is added to a list that will be processed by the World.
      /// Only a physics engine implementation should call this function.
      /// If you are unsure whether you should use this function, do not.
      /// \param[in] _entity Entity that has moved.
      public: void _AddDirty(Entity *_entity);

      /// \brief Get whether sensors have been initialized.
      /// \return True if sensors have been initialized.
      public: bool SensorsInitialized() const;

      /// \internal
      /// \brief Set whether sensors have been initialized. This should only
      /// be called by SensorManager.
      /// \param[in] _init True if sensors have been initialized.
      public: void _SetSensorsInitialized(const bool _init);

      /// \cond
      /// This is an internal function.
      /// \brief Get a model by id.
      /// Each Entity has a unique ID, this function finds a Model with
      /// a passed in _id.
      /// \param[in] _id The id of the Model
      /// \return A pointer to the model, or NULL if no Model was found.
      private: ModelPtr GetModelById(unsigned int _id);
      /// \endcond

      /// \brief Load all plugins.
      ///
      /// Load all plugins specified in the SDF for the model.
      private: void LoadPlugins();

      /// \brief Create and load all entities.
      /// \param[in] _sdf SDF element.
      /// \param[in] _parent Parent of the model to load.
      private: void LoadEntities(sdf::ElementPtr _sdf, BasePtr _parent);

      /// \brief Load a model.
      /// \param[in] _sdf SDF element containing the Model description.
      /// \param[in] _parent Parent of the model.
      /// \return Pointer to the newly created Model.
      private: ModelPtr LoadModel(sdf::ElementPtr _sdf, BasePtr _parent);

      /// \brief Load a light.
      /// \param[in] _sdf SDF element containing the Light description.
      /// \param[in] _parent Parent of the light.
      /// \return Pointer to the newly created Light.
      private: LightPtr LoadLight(const sdf::ElementPtr &_sdf,
          const BasePtr &_parent);

      /// \brief Load an actor.
      /// \param[in] _sdf SDF element containing the Actor description.
      /// \param[in] _parent Parent of the Actor.
      /// \return Pointer to the newly created Actor.
      private: ActorPtr LoadActor(sdf::ElementPtr _sdf, BasePtr _parent);

      /// \brief Load a road.
      /// \param[in] _sdf SDF element containing the Road description.
      /// \param[in] _parent Parent of the Road.
      /// \return Pointer to the newly created Road.
      private: RoadPtr LoadRoad(sdf::ElementPtr _sdf, BasePtr _parent);

      /// \brief Function to run physics. Used by physicsThread.
      private: void RunLoop();

      /// \brief Step the world once.
      private: void Step();

      /// \brief Step the world once by reading from a log file.
      private: void LogStep();

      /// \brief Update the world.
      private: void Update();

      /// \brief Pause callback.
      /// \param[in] _p True if paused.
      private: void OnPause(bool _p);

      /// \brief Step callback.
      private: void OnStep();

      /// \brief Called when a world control message is received.
      /// \param[in] _data The world control message.
      private: void OnControl(ConstWorldControlPtr &_data);

      /// \brief Called when log playback control message is received.
      /// \param[in] _data The log playback control message.
      private: void OnPlaybackControl(ConstLogPlaybackControlPtr &_data);

      /// \brief Called when a request message is received.
      /// \param[in] _msg The request message.
      private: void OnRequest(ConstRequestPtr &_msg);

      /// \brief Construct a scene message from the known world state
      /// \param[out] _scene Scene message to build.
      /// \param[in] _entity Pointer to entity from which to build the scene
      /// message.
      private: void BuildSceneMsg(msgs::Scene &_scene, BasePtr _entity);

      /// \brief Logs joint information.
      /// \param[in] _msg Incoming joint message.
      private: void JointLog(ConstJointPtr &_msg);

      /// \brief Called when a factory message is received.
      /// \param[in] _data The factory message.
      private: void OnFactoryMsg(ConstFactoryPtr &_data);

      /// \brief Called when a model message is received.
      /// \param[in] _msg The model message.
      private: void OnModelMsg(ConstModelPtr &_msg);

      /// \brief TBB version of model updating.
      private: void ModelUpdateTBB();

      /// \brief Single loop version of model updating.
      private: void ModelUpdateSingleLoop();

      /// \brief Helper function to load a plugin from SDF.
      /// \param[in] _sdf SDF plugin description.
      private: void LoadPlugin(sdf::ElementPtr _sdf);

      /// \brief Fills a model message with data from a model
      /// \param[out] _msg Model message to fill.
      /// \param[in] _model Pointer to the model to get the data from.
      private: void FillModelMsg(msgs::Model &_msg, ModelPtr _model);

      /// \brief Process all received entity messages.
      /// Must only be called from the World::ProcessMessages function.
      private: void ProcessEntityMsgs();

      /// \brief Process all received request messages.
      /// Must only be called from the World::ProcessMessages function.
      private: void ProcessRequestMsgs();

      /// \brief Process all received factory messages.
      /// Must only be called from the World::ProcessMessages function.
      private: void ProcessFactoryMsgs();

      /// \brief Process all received model messages.
      /// Must only be called from the World::ProcessMessages function.
      private: void ProcessModelMsgs();

      /// \brief Process all received light factory messages.
      /// Must only be called from the World::ProcessMessages function.
      private: void ProcessLightFactoryMsgs();

      /// \brief Process all received light modify messages.
      /// Must only be called from the World::ProcessMessages function.
      private: void ProcessLightModifyMsgs();

      /// \brief Log callback. This is where we write out state info.
      private: bool OnLog(std::ostringstream &_stream);

      /// \brief Process all incoming messages.
      private: void ProcessMessages();

      /// \brief Publish the world stats message.
      private: void PublishWorldStats();

      /// \brief Thread function for logging state data.
      private: void LogWorker();

      /// \brief Callback when a light message is received.
      /// \param[in] _msg Pointer to the light message.
      /// \deprecated Topic ~/light deprecated.
      /// See OnLightFactoryMsg which subscribes to ~/factory/light and
      /// OnLightModifyMsg which subscribes to ~/light/modify
      private: void OnLightMsg(ConstLightPtr &_msg);

      /// \brief Callback when a light message is received in the
      /// ~/factory/light topic.
      /// \param[in] _msg Pointer to the light message.
      private: void OnLightFactoryMsg(ConstLightPtr &_msg);

      /// \brief Callback when a light message is received in the
      /// ~/light/modify topic.
      /// \param[in] _msg Pointer to the light message.
      private: void OnLightModifyMsg(ConstLightPtr &_msg);

      /// \internal
      /// \brief Private data pointer.
      private: WorldPrivate *dataPtr;


      /// Friend DARTLink so that it has access to dataPtr->dirtyPoses
      private: friend class DARTLink;

      /// Friend SimbodyPhysics so that it has access to dataPtr->dirtyPoses
      private: friend class SimbodyPhysics;
    };
    /// \}
  }
}
#endif