diff --git a/README.md b/README.md index 8440554..b408e8a 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,45 @@ -tum_simulator ported to Hydro -============= +## tum_simulator ported to Kinetic. + +Tested on Ubuntu 16.04.1, ROS Kinetic and using Gazebo 7. + +These packages are used to simulate the flying robot Ardrone in ROS environment using gazebo simulator. Totally they are 4 packages. Their functions are descript as below: + +1. cvg_sim_gazebo: contains object models, sensor models, quadrocopter models, flying environment information and individual launch files for each objects and pure environment without any other objects. + +2. cvg_sim_gazebo_plugins: contains gazebo plugins for the quadrocopter model. quadrotor_simple_controller is used to control the robot motion and deliver navigation information, such as: /ardrone/navdata. Others are plugins for sensors in the quadrocopter, such as barometer sensor. + +3. message_to_tf: is a package used to create a ros node, which transfers the ros topic /ground_truth/state to a /tf topic. + +4. cvg_sim_msgs: contains message forms for the simulator. + +Some packages are based on the tu-darmstadt-ros-pkg by Stefan Kohlbrecher, TU Darmstadt. + + +How to run a simulation: + +0. Install dependencies: + + * Hector quadrotor: As an example you can install all hector suite running `sudo apt-get install ros--hector-*` + * ardrone_autonomy: run udo apt-get install ros--ardrone_autonomy` + +1. Download this `tum_simulator` package. + +2. Build packages with `catkin_make` in the workspace root folder. + +3. Start a ros master by typing the following command in console: `roscore` + +4. Run a simulation by executing a launch file in cvg_test_sim package: `roslaunch cvg_sim_gazebo ardrone_testworld.launch` + +5. You can manipulate the quadrocopter in gazebo simulator by downloading the ardrone_helpers package and using a joy-stick after calling this command:`roslaunch ardrone_joystick teleop.launch` + + +**Information for manipulation via joystick:** + +1. The `L1` button starts the quadrocopter. It also works as a deadman button so that the robot will land if you release it during flight. + +2. The left stick can be used to control the vx/vy-velocity. Keep in mind that these velocities are given in the local frame of the drone! + +3. The right stick controls the yaw-rate and the altitude. + +4. The select button can be used to switch between the two cameras. This can also be done by executing `rosservice call /ardrone/togglecam`. + diff --git a/cvg_sim_gazebo/launch/empty_world.launch b/cvg_sim_gazebo/launch/empty_world.launch index 95cc036..b857e6f 100644 --- a/cvg_sim_gazebo/launch/empty_world.launch +++ b/cvg_sim_gazebo/launch/empty_world.launch @@ -1,8 +1,8 @@ - + - + diff --git a/cvg_sim_gazebo/launch/land_station1.launch b/cvg_sim_gazebo/launch/land_station1.launch index 77f8cf0..69f1693 100644 --- a/cvg_sim_gazebo/launch/land_station1.launch +++ b/cvg_sim_gazebo/launch/land_station1.launch @@ -1,5 +1,5 @@ - + diff --git a/cvg_sim_gazebo/launch/land_station2.launch b/cvg_sim_gazebo/launch/land_station2.launch index 08d7d89..50389cd 100644 --- a/cvg_sim_gazebo/launch/land_station2.launch +++ b/cvg_sim_gazebo/launch/land_station2.launch @@ -1,5 +1,5 @@ - + diff --git a/cvg_sim_gazebo/launch/rolling_landscape_120m.launch b/cvg_sim_gazebo/launch/rolling_landscape_120m.launch index 4cd36dc..4adf9df 100644 --- a/cvg_sim_gazebo/launch/rolling_landscape_120m.launch +++ b/cvg_sim_gazebo/launch/rolling_landscape_120m.launch @@ -4,10 +4,10 @@ - + - + diff --git a/cvg_sim_gazebo/launch/spawn_quadrotor.launch b/cvg_sim_gazebo/launch/spawn_quadrotor.launch index a0c2368..4b00d85 100644 --- a/cvg_sim_gazebo/launch/spawn_quadrotor.launch +++ b/cvg_sim_gazebo/launch/spawn_quadrotor.launch @@ -10,7 +10,7 @@ - + - + diff --git a/cvg_sim_gazebo/launch/spawn_quadrotor_2.launch b/cvg_sim_gazebo/launch/spawn_quadrotor_2.launch index 4e91f5a..26087e7 100644 --- a/cvg_sim_gazebo/launch/spawn_quadrotor_2.launch +++ b/cvg_sim_gazebo/launch/spawn_quadrotor_2.launch @@ -4,10 +4,10 @@ - + - - + diff --git a/cvg_sim_gazebo/launch/test.launch b/cvg_sim_gazebo/launch/test.launch index fb83cae..5f5f854 100644 --- a/cvg_sim_gazebo/launch/test.launch +++ b/cvg_sim_gazebo/launch/test.launch @@ -4,10 +4,10 @@ - + - + diff --git a/cvg_sim_gazebo/package.xml b/cvg_sim_gazebo/package.xml index 7f2f044..491c3b4 100644 --- a/cvg_sim_gazebo/package.xml +++ b/cvg_sim_gazebo/package.xml @@ -42,7 +42,7 @@ catkin gazebo_ros gazebo_ros - cvg_sim_gazebo_plugins + diff --git a/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro index 158639a..fedfd07 100644 --- a/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro +++ b/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro @@ -3,11 +3,10 @@ +xmlns:xacro="http://ros.org/wiki/xacro"> - + diff --git a/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro index bdc5c23..6b40ec8 100644 --- a/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro +++ b/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro @@ -1,17 +1,22 @@ + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:xacro="http://ros.org/wiki/xacro"> - - + - + + + + + + + + + @@ -33,9 +38,7 @@ xmlns:xacro="http://ros.org/wiki/xacro" - - - + false diff --git a/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro index 99e7bbd..dbfc53f 100644 --- a/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro +++ b/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro @@ -3,42 +3,41 @@ +xmlns:xacro="http://ros.org/wiki/xacro"> - + - + - + - + - + - + diff --git a/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro b/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro index 3e88c19..f669823 100644 --- a/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro +++ b/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro @@ -2,15 +2,13 @@ +xmlns:xacro="http://ros.org/wiki/xacro"> - + - + @@ -92,7 +90,7 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" ardrone_base_${name}cam - true + false PR2/Blue diff --git a/cvg_sim_gazebo/urdf/sensors/hokuyo_utm30lx.urdf.xacro b/cvg_sim_gazebo/urdf/sensors/hokuyo_utm30lx.urdf.xacro index a575165..1e922de 100644 --- a/cvg_sim_gazebo/urdf/sensors/hokuyo_utm30lx.urdf.xacro +++ b/cvg_sim_gazebo/urdf/sensors/hokuyo_utm30lx.urdf.xacro @@ -2,12 +2,11 @@ +xmlns:xacro="http://ros.org/wiki/xacro"> + - + diff --git a/cvg_sim_gazebo/urdf/sensors/sonar_sensor.urdf.xacro b/cvg_sim_gazebo/urdf/sensors/sonar_sensor.urdf.xacro index 56d6e52..88f289f 100644 --- a/cvg_sim_gazebo/urdf/sensors/sonar_sensor.urdf.xacro +++ b/cvg_sim_gazebo/urdf/sensors/sonar_sensor.urdf.xacro @@ -2,9 +2,8 @@ +xmlns:xacro="http://ros.org/wiki/xacro"> + diff --git a/cvg_sim_gazebo_plugins/CMakeLists.txt b/cvg_sim_gazebo_plugins/CMakeLists.txt index d77a268..9569e27 100644 --- a/cvg_sim_gazebo_plugins/CMakeLists.txt +++ b/cvg_sim_gazebo_plugins/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 2.8.3) project(cvg_sim_gazebo_plugins) +set (CMAKE_CXX_STANDARD 11) + ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages @@ -86,7 +88,10 @@ generate_messages( ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include - LIBRARIES diffdrive_plugin_6w reset_plugin hector_gazebo_ros_imu hector_gazebo_ros_magnetic hector_gazebo_ros_gps hector_gazebo_ros_sonar hector_gazebo_ros_baro hector_gazebo_quadrotor_simple_controller hector_gazebo_quadrotor_state_controller + LIBRARIES + reset_plugin + hector_gazebo_ros_baro + hector_gazebo_quadrotor_simple_controller hector_gazebo_quadrotor_state_controller CATKIN_DEPENDS image_transport DEPENDS system_lib ) @@ -107,19 +112,9 @@ include_directories( ) ## Declare a cpp library -add_library(diffdrive_plugin_6w src/diffdrive_plugin_6w.cpp) -target_link_libraries(diffdrive_plugin_6w ${Boost_LIBRARIES}) add_library(reset_plugin src/reset_plugin.cpp) -add_library(hector_gazebo_ros_imu src/gazebo_ros_imu.cpp) -add_dependencies(hector_gazebo_ros_imu cvg_sim_gazebo_plugins_gencpp) -target_link_libraries(hector_gazebo_ros_imu ${Boost_LIBRARIES}) - -add_library(hector_gazebo_ros_magnetic src/gazebo_ros_magnetic.cpp) -add_library(hector_gazebo_ros_gps src/gazebo_ros_gps.cpp) -add_library(hector_gazebo_ros_sonar src/gazebo_ros_sonar.cpp) - add_library(hector_gazebo_ros_baro src/gazebo_ros_baro.cpp) add_dependencies(hector_gazebo_ros_baro cvg_sim_msgs_gencpp) diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/diffdrive_plugin_6w.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/diffdrive_plugin_6w.h deleted file mode 100644 index 31fc3a7..0000000 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/diffdrive_plugin_6w.h +++ /dev/null @@ -1,121 +0,0 @@ -/* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ -/* - * Desc: ROS interface to a Position2d controller for a Differential drive. - * Author: Daniel Hewlett (adapted from Nathan Koenig) - */ -#ifndef DIFFDRIVE_PLUGIN_HH -#define DIFFDRIVE_PLUGIN_HH - -#include - -#include "common/Plugin.hh" -#include "common/Time.hh" -//#include "physics/physics.h" -//#include "transport/TransportTypes.hh" -//#include "msgs/MessageTypes.hh" -//#include "common/Time.hh" -//#include "common/Events.hh" - -// ROS -#include -#include -#include -#include -#include - -// Custom Callback Queue -#include -#include - -// Boost -#include -#include - -namespace gazebo -{ - -class DiffDrivePlugin6W : public ModelPlugin -{ - -public: - DiffDrivePlugin6W(); - virtual ~DiffDrivePlugin6W(); - -protected: - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Reset(); - virtual void Update(); - -private: - void publish_odometry(); - void GetPositionCmd(); - - physics::LinkPtr link; - physics::WorldPtr world; - physics::JointPtr joints[6]; - - float wheelSep; - float wheelDiam; - float torque; - float wheelSpeed[2]; - - // Simulation time of the last update - common::Time prevUpdateTime; - - bool enableMotors; - float odomPose[3]; - float odomVel[3]; - - // ROS STUFF - ros::NodeHandle* rosnode_; - ros::Publisher pub_; - ros::Subscriber sub_; - tf::TransformBroadcaster *transform_broadcaster_; - nav_msgs::Odometry odom_; - std::string tf_prefix_; - - boost::mutex lock; - - std::string robotNamespace; - std::string topicName; - std::string linkName; - - // Custom Callback Queue - ros::CallbackQueue queue_; - boost::thread callback_queue_thread_; - void QueueThread(); - - // DiffDrive stuff - void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); - - float x_; - float rot_; - bool alive_; - - // Pointer to the update event connection - event::ConnectionPtr updateConnection; -}; - -} - -#endif - diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_gps.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_gps.h deleted file mode 100644 index 3c70110..0000000 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_gps.h +++ /dev/null @@ -1,94 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_GPS_H -#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_GPS_H - -#include "common/Plugin.hh" -#include "common/Time.hh" - -#include -#include -#include -#include - -namespace gazebo -{ - -class GazeboRosGps : public ModelPlugin -{ -public: - GazeboRosGps(); - virtual ~GazeboRosGps(); - -protected: - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Reset(); - virtual void Update(); - -private: - /// \brief The parent World - physics::WorldPtr world; - - /// \brief The link referred to by this plugin - physics::LinkPtr link; - - ros::NodeHandle* node_handle_; - ros::Publisher fix_publisher_; - ros::Publisher velocity_publisher_; - - sensor_msgs::NavSatFix fix_; - geometry_msgs::Vector3Stamped velocity_; - - std::string namespace_; - std::string link_name_; - std::string frame_id_; - std::string fix_topic_; - std::string velocity_topic_; - - double reference_latitude_; - double reference_longitude_; - double reference_heading_; - double reference_altitude_; - sensor_msgs::NavSatStatus::_status_type status_; - sensor_msgs::NavSatStatus::_service_type service_; - - SensorModel3 position_error_model_; - SensorModel3 velocity_error_model_; - - /// \brief save last_time - common::Time last_time; - common::Time update_period; - - // Pointer to the update event connection - event::ConnectionPtr updateConnection; -}; - -} // namespace gazebo - -#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_GPS_H diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_imu.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_imu.h deleted file mode 100644 index 351ac9f..0000000 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_imu.h +++ /dev/null @@ -1,165 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= -// This code is based on the original gazebo_ros_imu plugin by Sachin Chitta and John Hsu: -/* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ -/* - * Desc: 3D position interface. - * Author: Sachin Chitta and John Hsu - * Date: 10 June 2008 - * SVN: $Id$ - */ -//================================================================================================= - - -#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H -#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H - -// #define USE_CBQ -#ifdef USE_CBQ -#include -#include -#endif - -#include "common/Plugin.hh" -#include "common/Time.hh" - -#include -#include -#include -#include -#include -#include - -namespace gazebo -{ - class GazeboRosIMU : public ModelPlugin - { - public: - /// \brief Constructor - GazeboRosIMU(); - - /// \brief Destructor - virtual ~GazeboRosIMU(); - - protected: - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Reset(); - virtual void Update(); - - private: - /// \brief The parent World - physics::WorldPtr world; - - /// \brief The link referred to by this plugin - physics::LinkPtr link; - - /// \brief pointer to ros node - ros::NodeHandle* node_handle_; - ros::Publisher pub_; - - /// \brief ros message - sensor_msgs::Imu imuMsg; - - /// \brief store link name - std::string linkName; - - /// \brief frame id - std::string frameId; - - /// \brief topic name - std::string topicName; - - /// \brief Sensor models - SensorModel3 accelModel; - SensorModel3 rateModel; - SensorModel headingModel; - - /// \brief A mutex to lock access to fields that are used in message callbacks - boost::mutex lock; - - /// \brief save last_time - common::Time last_time; - common::Time update_period; - - /// \brief save current body/physics state - math::Quaternion orientation; - math::Vector3 velocity; - math::Vector3 accel; - math::Vector3 rate; - math::Vector3 gravity; - math::Vector3 gravity_body; - - /// \brief Gaussian noise generator - double GaussianKernel(double mu,double sigma); - - /// \brief for setting ROS name space - std::string robotNamespace; - - /// \brief call back when using service - bool ServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res); - ros::ServiceServer srv_; - std::string serviceName; - - /// \brief Bias service callbacks - bool SetAccelBiasCallback(cvg_sim_gazebo_plugins::SetBias::Request &req, cvg_sim_gazebo_plugins::SetBias::Response &res); - bool SetRateBiasCallback(cvg_sim_gazebo_plugins::SetBias::Request &req, cvg_sim_gazebo_plugins::SetBias::Response &res); - ros::ServiceServer accelBiasService; - ros::ServiceServer rateBiasService; - -#ifdef USE_CBQ - ros::CallbackQueue callback_queue_; - void CallbackQueueThread(); - boost::thread callback_queue_thread_; -#endif - - // Pointer to the update event connection - event::ConnectionPtr updateConnection; - }; -} - -#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_IMU_H diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_magnetic.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_magnetic.h deleted file mode 100644 index f24071f..0000000 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_magnetic.h +++ /dev/null @@ -1,88 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_MAGNETIC_H -#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_MAGNETIC_H - -#include "common/Plugin.hh" -#include "common/Time.hh" - -#include -#include -#include - -namespace gazebo -{ - -class GazeboRosMagnetic : public ModelPlugin -{ -public: - GazeboRosMagnetic(); - virtual ~GazeboRosMagnetic(); - -protected: - virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); - virtual void Reset(); - virtual void Update(); - -private: - /// \brief The parent World - physics::WorldPtr world; - - /// \brief The link referred to by this plugin - physics::LinkPtr link; - - ros::NodeHandle* node_handle_; - ros::Publisher publisher_; - - geometry_msgs::Vector3Stamped magnetic_field_; - gazebo::math::Vector3 magnetic_field_world_; - - std::string namespace_; - std::string topic_; - std::string link_name_; - std::string frame_id_; - - double magnitude_; - double reference_heading_; - double declination_; - double inclination_; - - SensorModel3 sensor_model_; - - /// \brief save last_time - common::Time last_time; - common::Time update_period; - - // Pointer to the update event connection - event::ConnectionPtr updateConnection; -}; - -} // namespace gazebo - -#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_MAGNETIC_H diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_sonar.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_sonar.h deleted file mode 100644 index 9c96c1f..0000000 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/gazebo_ros_sonar.h +++ /dev/null @@ -1,81 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -#ifndef HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H -#define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H - -#include "common/Plugin.hh" -#include "common/Time.hh" - -#include -#include - -#include -#include - -namespace gazebo -{ - -class GazeboRosSonar : public SensorPlugin -{ -public: - GazeboRosSonar(); - virtual ~GazeboRosSonar(); - -protected: - virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf); - virtual void Reset(); - virtual void Update(); - -private: - /// \brief The parent World - physics::WorldPtr world; - - sensors::RaySensorPtr sensor_; - - ros::NodeHandle* node_handle_; - ros::Publisher publisher_; - - sensor_msgs::Range range_; - - std::string namespace_; - std::string topic_; - std::string frame_id_; - - SensorModel sensor_model_; - - /// \brief save last_time - common::Time last_time; - - // Pointer to the update event connection - event::ConnectionPtr updateConnection; -}; - -} // namespace gazebo - -#endif // HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_SONAR_H diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/reset_plugin.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/reset_plugin.h index 2c37e02..97e064a 100644 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/reset_plugin.h +++ b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/reset_plugin.h @@ -29,7 +29,7 @@ #ifndef HECTOR_GAZEBO_PLUGINS_RESET_PLUGIN_H #define HECTOR_GAZEBO_PLUGINS_RESET_PLUGIN_H -#include "common/Plugin.hh" +#include "gazebo/common/Plugin.hh" #include diff --git a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h index 293ab58..e6dec0c 100644 --- a/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h +++ b/cvg_sim_gazebo_plugins/include/hector_gazebo_plugins/sensor_model.h @@ -30,7 +30,7 @@ #define HECTOR_GAZEBO_PLUGINS_SENSOR_MODEL_H #include -#include +#include namespace gazebo { @@ -97,10 +97,10 @@ void SensorModel_::Load(sdf::ElementPtr _sdf, const std::string& prefix) _gaussian_noise = _sdf->GetElement(prefix + "GaussianNoise"); } - if (_offset && !_offset->GetValue()->Get(offset)) offset = _offset->GetValueDouble(); - if (_drift && !_drift->GetValue()->Get(drift)) drift = _drift->GetValueDouble(); - if (_drift_frequency && !_drift_frequency->GetValue()->Get(drift_frequency)) drift_frequency = _drift_frequency->GetValueDouble(); - if (_gaussian_noise && !_gaussian_noise->GetValue()->Get(gaussian_noise)) gaussian_noise = _gaussian_noise->GetValueDouble(); + if (_offset && !_offset->GetValue()->Get(offset)) _offset->GetValue()->Get(offset); + if (_drift && !_drift->GetValue()->Get(drift)) _drift->GetValue()->Get(drift); + if (_drift_frequency && !_drift_frequency->GetValue()->Get(drift_frequency)) _drift_frequency->GetValue()->Get(drift_frequency); + if (_gaussian_noise && !_gaussian_noise->GetValue()->Get(gaussian_noise)) _gaussian_noise->GetValue()->Get(gaussian_noise); } namespace { diff --git a/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h b/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h index 6560900..ebb24c5 100644 --- a/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h +++ b/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_simple_controller.h @@ -46,7 +46,7 @@ #define HECTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H #include "gazebo/gazebo.hh" -#include "common/Plugin.hh" +#include "gazebo/common/Plugin.hh" #include #include diff --git a/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h b/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h index 6b945d2..9931c69 100644 --- a/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h +++ b/cvg_sim_gazebo_plugins/include/hector_quadrotor_controller/quadrotor_state_controller.h @@ -15,7 +15,7 @@ #define HECTOR_GAZEBO_PLUGINS_quadrotor_state_controller_H #include "gazebo/gazebo.hh" -#include "common/Plugin.hh" +#include "gazebo/common/Plugin.hh" #include #include diff --git a/cvg_sim_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h b/cvg_sim_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h index 62fdc65..8c19f9d 100644 --- a/cvg_sim_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h +++ b/cvg_sim_gazebo_plugins/include/hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h @@ -30,7 +30,7 @@ #define HECTOR_GAZEBO_PLUGINS_GAZEBO_ROS_BARO_H #include "gazebo/gazebo.hh" -#include "common/Plugin.hh" +#include "gazebo/common/Plugin.hh" #include #ifdef USE_MAV_MSGS diff --git a/cvg_sim_gazebo_plugins/src/diffdrive_plugin_6w.cpp b/cvg_sim_gazebo_plugins/src/diffdrive_plugin_6w.cpp deleted file mode 100644 index 53a68ff..0000000 --- a/cvg_sim_gazebo_plugins/src/diffdrive_plugin_6w.cpp +++ /dev/null @@ -1,339 +0,0 @@ -//================================================================================================= -// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Simulation, Systems Optimization and Robotics -// group, TU Darmstadt nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -/** - * Based on diffdrive_plugin by Nathan Koenig, Andrew Howard and Daniel Hewlett - */ - -#include -#include - -#include -#include "common/Events.hh" -#include "physics/physics.hh" - -#include -#include -#include -#include -#include -#include - -namespace gazebo { - -enum -{ - FRONT_LEFT, - FRONT_RIGHT, - MID_LEFT, - MID_RIGHT, - REAR_LEFT, - REAR_RIGHT, - NUM_WHEELS -}; - -// Constructor -DiffDrivePlugin6W::DiffDrivePlugin6W() -{ -} - -// Destructor -DiffDrivePlugin6W::~DiffDrivePlugin6W() -{ - event::Events::DisconnectWorldUpdateStart(updateConnection); - delete transform_broadcaster_; - rosnode_->shutdown(); - callback_queue_thread_.join(); - delete rosnode_; -} - -// Load the controller -void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - world = _model->GetWorld(); - - // load parameters - if (!_sdf->HasElement("robotNamespace")) - robotNamespace.clear(); - else - robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; - - if (!_sdf->HasElement("topicName")) - topicName = "cmd_vel"; - else - topicName = _sdf->GetElement("topicName")->GetValueString(); - - if (!_sdf->HasElement("bodyName")) - { - link = _model->GetLink(); - linkName = link->GetName(); - } - else { - linkName = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(_sdf->GetElement("bodyName")->GetValueString())); - } - - // assert that the body by linkName exists - if (!link) - { - ROS_FATAL("DiffDrivePlugin6W plugin error: bodyName: %s does not exist\n", linkName.c_str()); - return; - } - - if (_sdf->HasElement("frontLeftJoint")) joints[FRONT_LEFT] = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->GetValueString()); - if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->GetValueString()); - if (_sdf->HasElement("midLeftJoint")) joints[MID_LEFT] = _model->GetJoint(_sdf->GetElement("midLeftJoint")->GetValueString()); - if (_sdf->HasElement("midRightJoint")) joints[MID_RIGHT] = _model->GetJoint(_sdf->GetElement("midRightJoint")->GetValueString()); - if (_sdf->HasElement("rearLeftJoint")) joints[REAR_LEFT] = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->GetValueString()); - if (_sdf->HasElement("rearRightJoint")) joints[REAR_RIGHT] = _model->GetJoint(_sdf->GetElement("rearRightJoint")->GetValueString()); - - if (!joints[FRONT_LEFT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front left joint"); - if (!joints[FRONT_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get front right joint"); - if (!joints[MID_LEFT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid left joint"); - if (!joints[MID_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get mid right joint"); - if (!joints[REAR_LEFT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear left joint"); - if (!joints[REAR_RIGHT]) ROS_FATAL("diffdrive_plugin_6w error: The controller couldn't get rear right joint"); - - if (!_sdf->HasElement("wheelSeparation")) - wheelSep = 0.34; - else - wheelSep = _sdf->GetElement("wheelSeparation")->GetValueDouble(); - - if (!_sdf->HasElement("wheelDiameter")) - wheelDiam = 0.15; - else - wheelDiam = _sdf->GetElement("wheelDiameter")->GetValueDouble(); - - if (!_sdf->HasElement("torque")) - torque = 10.0; - else - torque = _sdf->GetElement("torque")->GetValueDouble(); - - // start ros node - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); - } - - rosnode_ = new ros::NodeHandle(robotNamespace); - - tf_prefix_ = tf::getPrefixParam(*rosnode_); - transform_broadcaster_ = new tf::TransformBroadcaster(); - - // ROS: Subscribe to the velocity command topic (usually "cmd_vel") - ros::SubscribeOptions so = - ros::SubscribeOptions::create(topicName, 1, - boost::bind(&DiffDrivePlugin6W::cmdVelCallback, this, _1), - ros::VoidPtr(), &queue_); - sub_ = rosnode_->subscribe(so); - pub_ = rosnode_->advertise("odom", 1); - - callback_queue_thread_ = boost::thread(boost::bind(&DiffDrivePlugin6W::QueueThread, this)); - - Reset(); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&DiffDrivePlugin6W::Update, this)); -} - -// Initialize the controller -void DiffDrivePlugin6W::Reset() -{ - enableMotors = true; - - for (size_t i = 0; i < 2; ++i){ - wheelSpeed[i] = 0; - } - - prevUpdateTime = world->GetSimTime(); - - x_ = 0; - rot_ = 0; - alive_ = true; - - // Reset odometric pose - odomPose[0] = 0.0; - odomPose[1] = 0.0; - odomPose[2] = 0.0; - - odomVel[0] = 0.0; - odomVel[1] = 0.0; - odomVel[2] = 0.0; -} - -// Update the controller -void DiffDrivePlugin6W::Update() -{ - // TODO: Step should be in a parameter of this function - double d1, d2; - double dr, da; - common::Time stepTime; - - GetPositionCmd(); - - //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime(); - stepTime = world->GetSimTime() - prevUpdateTime; - prevUpdateTime = world->GetSimTime(); - - // Distance travelled by front wheels - d1 = stepTime.Double() * wheelDiam / 2 * joints[MID_LEFT]->GetVelocity(0); - d2 = stepTime.Double() * wheelDiam / 2 * joints[MID_RIGHT]->GetVelocity(0); - - dr = (d1 + d2) / 2; - da = (d1 - d2) / wheelSep; - - // Compute odometric pose - odomPose[0] += dr * cos(odomPose[2]); - odomPose[1] += dr * sin(odomPose[2]); - odomPose[2] += da; - - // Compute odometric instantaneous velocity - odomVel[0] = dr / stepTime.Double(); - odomVel[1] = 0.0; - odomVel[2] = da / stepTime.Double(); - - if (enableMotors) - { - joints[FRONT_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0)); - joints[MID_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0)); - joints[REAR_LEFT]->SetVelocity(0, wheelSpeed[0] / (wheelDiam / 2.0)); - - joints[FRONT_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0)); - joints[MID_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0)); - joints[REAR_RIGHT]->SetVelocity(0, wheelSpeed[1] / (wheelDiam / 2.0)); - - joints[FRONT_LEFT]->SetMaxForce(0, torque); - joints[MID_LEFT]->SetMaxForce(0, torque); - joints[REAR_LEFT]->SetMaxForce(0, torque); - - joints[FRONT_RIGHT]->SetMaxForce(0, torque); - joints[MID_RIGHT]->SetMaxForce(0, torque); - joints[REAR_RIGHT]->SetMaxForce(0, torque); - } - - //publish_odometry(); -} - -// NEW: Now uses the target velocities from the ROS message, not the Iface -void DiffDrivePlugin6W::GetPositionCmd() -{ - lock.lock(); - - double vr, va; - - vr = x_; //myIface->data->cmdVelocity.pos.x; - va = -rot_; //myIface->data->cmdVelocity.yaw; - - //std::cout << "X: [" << x_ << "] ROT: [" << rot_ << "]" << std::endl; - - // Changed motors to be always on, which is probably what we want anyway - enableMotors = true; //myIface->data->cmdEnableMotors > 0; - - //std::cout << enableMotors << std::endl; - - wheelSpeed[0] = vr + va * wheelSep / 2; - wheelSpeed[1] = vr - va * wheelSep / 2; - - lock.unlock(); -} - -// NEW: Store the velocities from the ROS message -void DiffDrivePlugin6W::cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg) -{ - //std::cout << "BEGIN CALLBACK\n"; - - lock.lock(); - - x_ = cmd_msg->linear.x; - rot_ = cmd_msg->angular.z; - - lock.unlock(); - - //std::cout << "END CALLBACK\n"; -} - -// NEW: custom callback queue thread -void DiffDrivePlugin6W::QueueThread() -{ - static const double timeout = 0.01; - - while (alive_ && rosnode_->ok()) - { - // std::cout << "CALLING STUFF\n"; - queue_.callAvailable(ros::WallDuration(timeout)); - } -} - -// NEW: Update this to publish odometry topic -void DiffDrivePlugin6W::publish_odometry() -{ - // get current time - ros::Time current_time_((world->GetSimTime()).sec, (world->GetSimTime()).nsec); - - // getting data for base_footprint to odom transform - math::Pose pose = link->GetWorldPose(); - math::Vector3 velocity = link->GetWorldLinearVel(); - math::Vector3 angular_velocity = link->GetWorldAngularVel(); - - tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w); - tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z); - tf::Transform base_footprint_to_odom(qt, vt); - - transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_odom, - current_time_, - "odom", - "base_footprint")); - - // publish odom topic - odom_.pose.pose.position.x = pose.pos.x; - odom_.pose.pose.position.y = pose.pos.y; - - odom_.pose.pose.orientation.x = pose.rot.x; - odom_.pose.pose.orientation.y = pose.rot.y; - odom_.pose.pose.orientation.z = pose.rot.z; - odom_.pose.pose.orientation.w = pose.rot.w; - - odom_.twist.twist.linear.x = velocity.x; - odom_.twist.twist.linear.y = velocity.y; - odom_.twist.twist.angular.z = angular_velocity.z; - - odom_.header.frame_id = tf::resolve(tf_prefix_, "odom"); - odom_.child_frame_id = "base_footprint"; - odom_.header.stamp = current_time_; - - pub_.publish(odom_); -} - -GZ_REGISTER_MODEL_PLUGIN(DiffDrivePlugin6W) - -} // namespace gazebo diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_baro.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_baro.cpp index 3876a5b..14453cd 100644 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_baro.cpp +++ b/cvg_sim_gazebo_plugins/src/gazebo_ros_baro.cpp @@ -27,8 +27,8 @@ //================================================================================================= #include -#include "common/Events.hh" -#include "physics/physics.hh" +#include "gazebo/common/Events.hh" +#include "gazebo/physics/physics.hh" static const double DEFAULT_ELEVATION = 0.0; static const double DEFAULT_QNH = 1013.25; @@ -43,7 +43,7 @@ GazeboRosBaro::GazeboRosBaro() // Destructor GazeboRosBaro::~GazeboRosBaro() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); delete node_handle_; @@ -59,7 +59,7 @@ void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("robotNamespace")) namespace_.clear(); else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString() + "/"; if (!_sdf->HasElement("bodyName")) { @@ -67,8 +67,8 @@ void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) link_name_ = link->GetName(); } else { - link_name_ = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(link_name_)); + link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString(); + link = boost::dynamic_pointer_cast(world->GetEntity(link_name_)); } if (!link) @@ -78,33 +78,33 @@ void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) } double update_rate = 0.0; - if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble(); + if (_sdf->HasElement("updateRate")) _sdf->GetElement("updateRate")->GetValue()->Get(update_rate); update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0; if (!_sdf->HasElement("frameId")) frame_id_ = link->GetName(); else - frame_id_ = _sdf->GetElement("frameId")->GetValueString(); + frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString(); if (!_sdf->HasElement("topicName")) height_topic_ = "pressure_height"; else - height_topic_ = _sdf->GetElement("topicName")->GetValueString(); + height_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString(); if (!_sdf->HasElement("altimeterTopicName")) altimeter_topic_ = "altimeter"; else - altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->GetValueString(); + altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->GetValue()->GetAsString(); if (!_sdf->HasElement("elevation")) elevation_ = DEFAULT_ELEVATION; else - elevation_ = _sdf->GetElement("elevation")->GetValueDouble(); + _sdf->GetElement("elevation")->GetValue()->Get(elevation_); if (!_sdf->HasElement("qnh")) qnh_ = DEFAULT_QNH; else - qnh_ = _sdf->GetElement("qnh")->GetValueDouble(); + _sdf->GetElement("qnh")->GetValue()->Get(qnh_); sensor_model_.Load(_sdf); height_.header.frame_id = frame_id_; diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_gps.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_gps.cpp deleted file mode 100644 index a13f18d..0000000 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_gps.cpp +++ /dev/null @@ -1,203 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -#include -#include "common/Events.hh" -#include "physics/physics.hh" - -static const double EARTH_RADIUS = 6371000.0; -static const double DEFAULT_REFERENCE_LATITUDE = 49.9; -static const double DEFAULT_REFERENCE_LONGITUDE = 8.9; -static const double DEFAULT_REFERENCE_HEADING = 0.0; -static const double DEFAULT_REFERENCE_ALTITUDE = 0.0; - -namespace gazebo { - -GazeboRosGps::GazeboRosGps() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosGps::~GazeboRosGps() -{ - event::Events::DisconnectWorldUpdateStart(updateConnection); - node_handle_->shutdown(); - delete node_handle_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - world = _model->GetWorld(); - - // load parameters - if (!_sdf->HasElement("robotNamespace")) - namespace_.clear(); - else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; - - if (!_sdf->HasElement("bodyName")) - { - link = _model->GetLink(); - link_name_ = link->GetName(); - } - else { - link_name_ = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(link_name_)); - } - - if (!link) - { - ROS_FATAL("GazeboRosGps plugin error: bodyName: %s does not exist\n", link_name_.c_str()); - return; - } - - double update_rate = 4.0; - if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble(); - update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0; - - if (!_sdf->HasElement("frameId")) - frame_id_ = link_name_; - else - frame_id_ = _sdf->GetElement("frameId")->GetValueString(); - - if (!_sdf->HasElement("topicName")) - fix_topic_ = "fix"; - else - fix_topic_ = _sdf->GetElement("topicName")->GetValueString(); - - if (!_sdf->HasElement("velocityTopicName")) - velocity_topic_ = "fix_velocity"; - else - velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValueString(); - - if (!_sdf->HasElement("referenceLatitude")) - reference_latitude_ = DEFAULT_REFERENCE_LATITUDE; - else - reference_latitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble(); - - if (!_sdf->HasElement("referenceLongitude")) - reference_longitude_ = DEFAULT_REFERENCE_LONGITUDE; - else - reference_longitude_ = _sdf->GetElement("referenceLongitude")->GetValueDouble(); - - if (!_sdf->HasElement("referenceHeading")) - reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0; - else - reference_heading_ = _sdf->GetElement("referenceLatitude")->GetValueDouble() * M_PI/180.0; - - if (!_sdf->HasElement("referenceAltitude")) - reference_altitude_ = DEFAULT_REFERENCE_ALTITUDE; - else - reference_altitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble(); - - if (!_sdf->HasElement("status")) - status_ = sensor_msgs::NavSatStatus::STATUS_FIX; - else - status_ = _sdf->GetElement("status")->GetValueUInt(); - - if (!_sdf->HasElement("service")) - service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; - else - service_ = _sdf->GetElement("service")->GetValueUInt(); - - fix_.header.frame_id = frame_id_; - fix_.status.status = status_; - fix_.status.service = service_; - velocity_.header.frame_id = frame_id_; - - position_error_model_.Load(_sdf); - velocity_error_model_.Load(_sdf, "velocity"); - - // start ros node - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); - } - - node_handle_ = new ros::NodeHandle(namespace_); - fix_publisher_ = node_handle_->advertise(fix_topic_, 10); - velocity_publisher_ = node_handle_->advertise(velocity_topic_, 10); - - Reset(); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosGps::Update, this)); -} - -void GazeboRosGps::Reset() -{ - last_time = world->GetSimTime(); - - position_error_model_.reset(); - velocity_error_model_.reset(); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosGps::Update() -{ - common::Time sim_time = world->GetSimTime(); - double dt = (sim_time - last_time).Double(); - if (last_time + update_period > sim_time) return; - - math::Pose pose = link->GetWorldPose(); - - gazebo::math::Vector3 velocity = velocity_error_model_(link->GetWorldLinearVel(), dt); - position_error_model_.setCurrentDrift(position_error_model_.getCurrentDrift() + velocity_error_model_.getCurrentError() * dt); - gazebo::math::Vector3 position = position_error_model_(pose.pos, dt); - - fix_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec); - velocity_.header.stamp = fix_.header.stamp; - - fix_.latitude = reference_latitude_ + ( cos(reference_heading_) * position.x + sin(reference_heading_) * position.y) / EARTH_RADIUS * 180.0/M_PI; - fix_.longitude = reference_longitude_ - (-sin(reference_heading_) * position.x + cos(reference_heading_) * position.y) / EARTH_RADIUS * 180.0/M_PI * cos(fix_.latitude); - fix_.altitude = reference_altitude_ + position.z; - fix_.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN; - velocity_.vector.x = cos(reference_heading_) * velocity.x + sin(reference_heading_) * velocity.y; - velocity_.vector.y = -sin(reference_heading_) * velocity.x + cos(reference_heading_) * velocity.y; - velocity_.vector.z = velocity.z; - - fix_publisher_.publish(fix_); - velocity_publisher_.publish(velocity_); - - // save last time stamp - last_time = sim_time; -} - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboRosGps) - -} // namespace gazebo diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp deleted file mode 100644 index 4eb1ccf..0000000 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp +++ /dev/null @@ -1,369 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= -// This code is based on the original gazebo_ros_imu plugin by Sachin Chitta and John Hsu: -/* - * Gazebo - Outdoor Multi-Robot Simulator - * Copyright (C) 2003 - * Nate Koenig & Andrew Howard - * - * This program is free software; you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation; either version 2 of the License, or - * (at your option) any later version. - * - * This program is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with this program; if not, write to the Free Software - * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA - * - */ -/* - * Desc: 3D position interface. - * Author: Sachin Chitta and John Hsu - * Date: 10 June 2008 - * SVN: $Id$ - */ -//================================================================================================= - -#include -#include "common/Events.hh" -#include "physics/physics.hh" - -namespace gazebo -{ - -// #define DEBUG_OUTPUT -#ifdef DEBUG_OUTPUT - #include - static ros::Publisher debugPublisher; -#endif // DEBUG_OUTPUT - -//////////////////////////////////////////////////////////////////////////////// -// Constructor -GazeboRosIMU::GazeboRosIMU() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosIMU::~GazeboRosIMU() -{ - event::Events::DisconnectWorldUpdateStart(updateConnection); - - node_handle_->shutdown(); -#ifdef USE_CBQ - callback_queue_thread_.join(); -#endif - delete node_handle_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - // Get the world name. - world = _model->GetWorld(); - - // load parameters - if (!_sdf->HasElement("robotNamespace")) - robotNamespace.clear(); - else - robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; - - if (!_sdf->HasElement("bodyName")) - { - link = _model->GetLink(); - linkName = link->GetName(); - } - else { - linkName = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(linkName)); - } - - // assert that the body by linkName exists - if (!link) - { - ROS_FATAL("GazeboRosIMU plugin error: bodyName: %s does not exist\n", linkName.c_str()); - return; - } - - double update_rate = 0.0; - if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble(); - update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0; - - if (!_sdf->HasElement("frameId")) - frameId = linkName; - else - frameId = _sdf->GetElement("frameId")->GetValueString(); - - if (!_sdf->HasElement("topicName")) - topicName = "imu"; - else - topicName = _sdf->GetElement("topicName")->GetValueString(); - - if (!_sdf->HasElement("serviceName")) - serviceName = topicName + "/calibrate"; - else - serviceName = _sdf->GetElement("serviceName")->GetValueString(); - - accelModel.Load(_sdf, "accel"); - rateModel.Load(_sdf, "rate"); - headingModel.Load(_sdf, "heading"); - - // also use old configuration variables from gazebo_ros_imu - if (_sdf->HasElement("gaussianNoise")) { - double gaussianNoise = _sdf->GetElement("gaussianNoise")->GetValueDouble(); - if (gaussianNoise != 0.0) { - accelModel.gaussian_noise = gaussianNoise; - rateModel.gaussian_noise = gaussianNoise; - } - } - - if (_sdf->HasElement("rpyOffset")) { - sdf::Vector3 sdfVec = _sdf->GetElement("rpyOffset")->GetValueVector3(); - math::Vector3 rpyOffset = math::Vector3(sdfVec.x, sdfVec.y, sdfVec.z); - if (accelModel.offset.y == 0.0 && rpyOffset.x != 0.0) accelModel.offset.y = -rpyOffset.x * 9.8065; - if (accelModel.offset.x == 0.0 && rpyOffset.y != 0.0) accelModel.offset.x = rpyOffset.y * 9.8065; - if (headingModel.offset == 0.0 && rpyOffset.z != 0.0) headingModel.offset = rpyOffset.z; - } - - // fill in constant covariance matrix - imuMsg.angular_velocity_covariance[0] = rateModel.gaussian_noise.x*rateModel.gaussian_noise.x; - imuMsg.angular_velocity_covariance[4] = rateModel.gaussian_noise.y*rateModel.gaussian_noise.y; - imuMsg.angular_velocity_covariance[8] = rateModel.gaussian_noise.z*rateModel.gaussian_noise.z; - imuMsg.linear_acceleration_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x; - imuMsg.linear_acceleration_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y; - imuMsg.linear_acceleration_covariance[8] = accelModel.gaussian_noise.z*accelModel.gaussian_noise.z; - - // start ros node - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); - } - - node_handle_ = new ros::NodeHandle(robotNamespace); - - // if topic name specified as empty, do not publish (then what is this plugin good for?) - if (!topicName.empty()) - pub_ = node_handle_->advertise(topicName, 1); - -#ifdef DEBUG_OUTPUT - debugPublisher = rosnode_->advertise(topicName + "/pose", 10); -#endif // DEBUG_OUTPUT - - // advertise services for calibration and bias setting - if (!serviceName.empty()) - srv_ = node_handle_->advertiseService(serviceName, &GazeboRosIMU::ServiceCallback, this); - - accelBiasService = node_handle_->advertiseService(topicName + "/set_accel_bias", &GazeboRosIMU::SetAccelBiasCallback, this); - rateBiasService = node_handle_->advertiseService(topicName + "/set_rate_bias", &GazeboRosIMU::SetRateBiasCallback, this); - -#ifdef USE_CBQ - // start custom queue for imu - callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosIMU::CallbackQueueThread,this ) ); -#endif - - Reset(); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosIMU::Update, this)); -} - -void GazeboRosIMU::Reset() -{ - last_time = world->GetSimTime(); - orientation = math::Quaternion(); - velocity = 0.0; - accel = 0.0; - - accelModel.reset(); - rateModel.reset(); - headingModel.reset(); -} - -//////////////////////////////////////////////////////////////////////////////// -// returns true always, imu is always calibrated in sim -bool GazeboRosIMU::ServiceCallback(std_srvs::Empty::Request &req, - std_srvs::Empty::Response &res) -{ - boost::mutex::scoped_lock scoped_lock(lock); - rateModel.reset(); - return true; -} - -bool GazeboRosIMU::SetAccelBiasCallback(cvg_sim_gazebo_plugins::SetBias::Request &req, cvg_sim_gazebo_plugins::SetBias::Response &res) -{ - boost::mutex::scoped_lock scoped_lock(lock); - accelModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z)); - return true; -} - -bool GazeboRosIMU::SetRateBiasCallback(cvg_sim_gazebo_plugins::SetBias::Request &req, cvg_sim_gazebo_plugins::SetBias::Response &res) -{ - boost::mutex::scoped_lock scoped_lock(lock); - rateModel.reset(math::Vector3(req.bias.x, req.bias.y, req.bias.z)); - return true; -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosIMU::Update() -{ - // Get Time Difference dt - common::Time cur_time = world->GetSimTime(); - double dt = (cur_time - last_time).Double(); - if (last_time + update_period > cur_time) return; - - boost::mutex::scoped_lock scoped_lock(lock); - - // Get Pose/Orientation - math::Pose pose = link->GetWorldPose(); - - // get Acceleration and Angular Rates - // the result of GetRelativeLinearAccel() seems to be unreliable (sum of forces added during the current simulation step)? - //accel = myBody->GetRelativeLinearAccel(); // get acceleration in body frame - math::Vector3 temp = link->GetWorldLinearVel(); // get velocity in world frame - accel = pose.rot.RotateVectorReverse((temp - velocity) / dt); - velocity = temp; - - // GetRelativeAngularVel() sometimes return nan? - //rate = link->GetRelativeAngularVel(); // get angular rate in body frame - math::Quaternion delta = pose.rot - orientation; - orientation = pose.rot; - rate.x = 2.0 * (-orientation.x * delta.w + orientation.w * delta.x + orientation.z * delta.y - orientation.y * delta.z) / dt; - rate.y = 2.0 * (-orientation.y * delta.w - orientation.z * delta.x + orientation.w * delta.y + orientation.x * delta.z) / dt; - rate.z = 2.0 * (-orientation.z * delta.w + orientation.y * delta.x - orientation.x * delta.y + orientation.w * delta.z) / dt; - - // get Gravity - gravity = world->GetPhysicsEngine()->GetGravity(); - gravity_body = orientation.RotateVectorReverse(gravity); - double gravity_length = gravity.GetLength(); - ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "gravity_world = [%g %g %g]", gravity.x, gravity.y, gravity.z); - - // add gravity vector to body acceleration - accel = accel - gravity_body; - - // update sensor models - accel = accel + accelModel.update(dt); - rate = rate + rateModel.update(dt); - headingModel.update(dt); - ROS_DEBUG_NAMED("hector_gazebo_ros_imu", "Current errors: accel = [%g %g %g], rate = [%g %g %g], heading = %g", - accelModel.getCurrentError().x, accelModel.getCurrentError().y, accelModel.getCurrentError().z, - rateModel.getCurrentError().x, rateModel.getCurrentError().y, rateModel.getCurrentError().z, - headingModel.getCurrentError()); - - // apply offset error to orientation (pseudo AHRS) - double normalization_constant = (gravity_body + accelModel.getCurrentError()).GetLength() * gravity_body.GetLength(); - double cos_alpha = (gravity_body + accelModel.getCurrentError()).Dot(gravity_body)/normalization_constant; - math::Vector3 normal_vector(gravity_body.Cross(accelModel.getCurrentError())); - normal_vector *= sqrt((1 - cos_alpha)/2)/normalization_constant; - math::Quaternion attitudeError(sqrt((1 + cos_alpha)/2), normal_vector.x, normal_vector.y, normal_vector.z); - math::Quaternion headingError(cos(headingModel.getCurrentError()/2),0,0,sin(headingModel.getCurrentError()/2)); - pose.rot = attitudeError * pose.rot * headingError; - - // copy data into pose message - imuMsg.header.frame_id = frameId; - imuMsg.header.stamp.sec = cur_time.sec; - imuMsg.header.stamp.nsec = cur_time.nsec; - - // orientation quaternion - imuMsg.orientation.x = pose.rot.x; - imuMsg.orientation.y = pose.rot.y; - imuMsg.orientation.z = pose.rot.z; - imuMsg.orientation.w = pose.rot.w; - - // pass angular rates - imuMsg.angular_velocity.x = rate.x; - imuMsg.angular_velocity.y = rate.y; - imuMsg.angular_velocity.z = rate.z; - - // pass accelerations - imuMsg.linear_acceleration.x = accel.x; - imuMsg.linear_acceleration.y = accel.y; - imuMsg.linear_acceleration.z = accel.z; - - // fill in covariance matrix - imuMsg.orientation_covariance[8] = headingModel.gaussian_noise*headingModel.gaussian_noise; - if (gravity_length > 0.0) { - imuMsg.orientation_covariance[0] = accelModel.gaussian_noise.x*accelModel.gaussian_noise.x/(gravity_length*gravity_length); - imuMsg.orientation_covariance[4] = accelModel.gaussian_noise.y*accelModel.gaussian_noise.y/(gravity_length*gravity_length); - } else { - imuMsg.orientation_covariance[0] = -1; - imuMsg.orientation_covariance[4] = -1; - } - - // publish to ros - pub_.publish(imuMsg); - - // debug output -#ifdef DEBUG_OUTPUT - if (debugPublisher) { - geometry_msgs::PoseStamped debugPose; - debugPose.header = imuMsg.header; - debugPose.header.frame_id = "/map"; - debugPose.pose.orientation.w = imuMsg.orientation.w; - debugPose.pose.orientation.x = imuMsg.orientation.x; - debugPose.pose.orientation.y = imuMsg.orientation.y; - debugPose.pose.orientation.z = imuMsg.orientation.z; - math::Pose pose = link->GetWorldPose(); - debugPose.pose.position.x = pose.pos.x; - debugPose.pose.position.y = pose.pos.y; - debugPose.pose.position.z = pose.pos.z; - debugPublisher.publish(debugPose); - } -#endif // DEBUG_OUTPUT - - // save last time stamp - last_time = cur_time; -} - -#ifdef USE_CBQ -void GazeboRosIMU::CallbackQueueThread() -{ - static const double timeout = 0.01; - - while (rosnode_->ok()) - { - callback_queue_.callAvailable(ros::WallDuration(timeout)); - } -} -#endif - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboRosIMU) - -} // namespace gazebo diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp deleted file mode 100644 index da62df4..0000000 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp +++ /dev/null @@ -1,174 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -#include -#include "common/Events.hh" -#include "physics/physics.hh" - -static const double DEFAULT_MAGNITUDE = 1.0; -static const double DEFAULT_REFERENCE_HEADING = 0.0; -static const double DEFAULT_DECLINATION = 0.0; -static const double DEFAULT_INCLINATION = 60.0; - -namespace gazebo { - -GazeboRosMagnetic::GazeboRosMagnetic() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosMagnetic::~GazeboRosMagnetic() -{ - event::Events::DisconnectWorldUpdateStart(updateConnection); - - node_handle_->shutdown(); - delete node_handle_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) -{ - world = _model->GetWorld(); - - // load parameters - if (!_sdf->HasElement("robotNamespace")) - namespace_.clear(); - else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; - - if (!_sdf->HasElement("topicName")) - topic_ = "magnetic"; - else - topic_ = _sdf->GetElement("topicName")->GetValueString(); - - if (!_sdf->HasElement("bodyName")) - { - link = _model->GetLink(); - link_name_ = link->GetName(); - } - else { - link_name_ = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(link_name_)); - } - - if (!link) - { - ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str()); - return; - } - - double update_rate = 0.0; - if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble(); - update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0; - - if (!_sdf->HasElement("frameId")) - frame_id_ = link_name_; - else - frame_id_ = _sdf->GetElement("frameId")->GetValueString(); - - if (!_sdf->HasElement("magnitude")) - magnitude_ = DEFAULT_MAGNITUDE; - else - magnitude_ = _sdf->GetElement("magnitude")->GetValueDouble(); - - if (!_sdf->HasElement("referenceHeading")) - reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0; - else - reference_heading_ = _sdf->GetElement("referenceHeading")->GetValueDouble() * M_PI/180.0; - - if (!_sdf->HasElement("declination")) - declination_ = DEFAULT_DECLINATION * M_PI/180.0; - else - declination_ = _sdf->GetElement("declination")->GetValueDouble() * M_PI/180.0; - - if (!_sdf->HasElement("inclination")) - inclination_ = DEFAULT_INCLINATION * M_PI/180.0; - else - inclination_ = _sdf->GetElement("inclination")->GetValueDouble() * M_PI/180.0; - - // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings - magnetic_field_.header.frame_id = frame_id_; - magnetic_field_world_.x = magnitude_ * cos(inclination_) * cos(reference_heading_ - declination_); - magnetic_field_world_.y = magnitude_ * sin(reference_heading_ - declination_); - magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_); - - sensor_model_.Load(_sdf); - - // start ros node - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); - } - - node_handle_ = new ros::NodeHandle(namespace_); - publisher_ = node_handle_->advertise(topic_, 1); - - Reset(); - - // New Mechanism for Updating every World Cycle - // Listen to the update event. This event is broadcast every - // simulation iteration. - updateConnection = event::Events::ConnectWorldUpdateBegin( - boost::bind(&GazeboRosMagnetic::Update, this)); -} - -void GazeboRosMagnetic::Reset() -{ - sensor_model_.reset(); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosMagnetic::Update() -{ - common::Time sim_time = world->GetSimTime(); - double dt = (sim_time - last_time).Double(); - if (last_time + update_period > sim_time) return; - - math::Pose pose = link->GetWorldPose(); - math::Vector3 field = sensor_model_(pose.rot.RotateVectorReverse(magnetic_field_world_), dt); - - magnetic_field_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec); - magnetic_field_.vector.x = field.x; - magnetic_field_.vector.y = field.y; - magnetic_field_.vector.z = field.z; - - publisher_.publish(magnetic_field_); - - // save last time stamp - last_time = sim_time; -} - -// Register this plugin with the simulator -GZ_REGISTER_MODEL_PLUGIN(GazeboRosMagnetic) - -} // namespace gazebo diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp deleted file mode 100644 index fd6bd6c..0000000 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp +++ /dev/null @@ -1,154 +0,0 @@ -//================================================================================================= -// Copyright (c) 2012, Johannes Meyer, TU Darmstadt -// All rights reserved. - -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// * Neither the name of the Flight Systems and Automatic Control group, -// TU Darmstadt, nor the names of its contributors may be used to -// endorse or promote products derived from this software without -// specific prior written permission. - -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY -// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -//================================================================================================= - -#include -#include "common/Events.hh" -#include "physics/physics.hh" -#include "sensors/RaySensor.hh" - -#include - -namespace gazebo { - -GazeboRosSonar::GazeboRosSonar() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -// Destructor -GazeboRosSonar::~GazeboRosSonar() -{ - sensor_->SetActive(false); - event::Events::DisconnectWorldUpdateStart(updateConnection); - node_handle_->shutdown(); - delete node_handle_; -} - -//////////////////////////////////////////////////////////////////////////////// -// Load the controller -void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) -{ - // Get then name of the parent sensor - sensor_ = boost::shared_dynamic_cast(_sensor); - if (!sensor_) - { - gzthrow("GazeboRosSonar requires a Ray Sensor as its parent"); - return; - } - - // Get the world name. - std::string worldName = sensor_->GetWorldName(); - world = physics::get_world(worldName); - - // load parameters - if (!_sdf->HasElement("robotNamespace")) - namespace_.clear(); - else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; - - if (!_sdf->HasElement("frameId")) - frame_id_ = ""; - else - frame_id_ = _sdf->GetElement("frameId")->GetValueString(); - - if (!_sdf->HasElement("topicName")) - topic_ = "sonar"; - else - topic_ = _sdf->GetElement("topicName")->GetValueString(); - - sensor_model_.Load(_sdf); - - range_.header.frame_id = frame_id_; - range_.radiation_type = sensor_msgs::Range::ULTRASOUND; - range_.field_of_view = std::min(fabs((sensor_->GetAngleMax() - sensor_->GetAngleMin()).Radian()), fabs((sensor_->GetVerticalAngleMax() - sensor_->GetVerticalAngleMin()).Radian())); - range_.max_range = sensor_->GetRangeMax(); - range_.min_range = sensor_->GetRangeMin(); - - // start ros node - if (!ros::isInitialized()) - { - int argc = 0; - char** argv = NULL; - ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); - } - - node_handle_ = new ros::NodeHandle(namespace_); - publisher_ = node_handle_->advertise(topic_, 1); - - Reset(); - updateConnection = sensor_->GetLaserShape()->ConnectNewLaserScans( - boost::bind(&GazeboRosSonar::Update, this)); - - // activate RaySensor - sensor_->SetActive(true); -} - -void GazeboRosSonar::Reset() -{ - sensor_model_.reset(); -} - -//////////////////////////////////////////////////////////////////////////////// -// Update the controller -void GazeboRosSonar::Update() -{ - common::Time sim_time = world->GetSimTime(); - double dt = (sim_time - last_time).Double(); -// if (last_time + updatePeriod > sim_time) return; - - // activate RaySensor if it is not yet active - if (!sensor_->IsActive()) sensor_->SetActive(true); - - range_.header.stamp.sec = (world->GetSimTime()).sec; - range_.header.stamp.nsec = (world->GetSimTime()).nsec; - - // find ray with minimal range - range_.range = std::numeric_limits::max(); - int num_ranges = sensor_->GetLaserShape()->GetSampleCount() * sensor_->GetLaserShape()->GetVerticalSampleCount(); - for(int i = 0; i < num_ranges; ++i) { - double ray = sensor_->GetLaserShape()->GetRange(i); - if (ray < range_.range) range_.range = ray; - } - - // add Gaussian noise (and limit to min/max range) - if (range_.range < range_.max_range) { - range_.range += sensor_model_.update(dt); - if (range_.range < range_.min_range) range_.range = range_.min_range; - if (range_.range > range_.max_range) range_.range = range_.max_range; - } - - publisher_.publish(range_); - - // save last time stamp - last_time = sim_time; -} - -// Register this plugin with the simulator -GZ_REGISTER_SENSOR_PLUGIN(GazeboRosSonar) - -} // namespace gazebo diff --git a/cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp b/cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp index 5e0dc65..d13933d 100644 --- a/cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp +++ b/cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp @@ -41,8 +41,8 @@ * */ #include -#include "common/Events.hh" -#include "physics/physics.hh" +#include "gazebo/common/Events.hh" +#include "gazebo/physics/physics.hh" #include #include @@ -58,7 +58,7 @@ GazeboQuadrotorSimpleController::GazeboQuadrotorSimpleController() // Destructor GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); delete node_handle_; @@ -74,27 +74,27 @@ void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::Elemen if (!_sdf->HasElement("robotNamespace")) namespace_.clear(); else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString() + "/"; if (!_sdf->HasElement("topicName")) velocity_topic_ = "cmd_vel"; else - velocity_topic_ = _sdf->GetElement("topicName")->GetValueString(); + velocity_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString(); if (!_sdf->HasElement("navdataTopic")) navdata_topic_ = "/ardrone/navdata"; else - navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValueString(); + navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("imuTopic")) imu_topic_.clear(); else - imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString(); + imu_topic_ = _sdf->GetElement("imuTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("stateTopic")) state_topic_.clear(); else - state_topic_ = _sdf->GetElement("stateTopic")->GetValueString(); + state_topic_ = _sdf->GetElement("stateTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("bodyName")) { @@ -102,8 +102,8 @@ void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::Elemen link_name_ = link->GetName(); } else { - link_name_ = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(link_name_)); + link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString(); + link = boost::dynamic_pointer_cast(world->GetEntity(link_name_)); } if (!link) @@ -115,23 +115,23 @@ void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::Elemen if (!_sdf->HasElement("maxForce")) max_force_ = -1; else - max_force_ = _sdf->GetElement("maxForce")->GetValueDouble(); + _sdf->GetElement("maxForce")->GetValue()->Get(max_force_); if (!_sdf->HasElement("motionSmallNoise")) motion_small_noise_ = 0; else - motion_small_noise_ = _sdf->GetElement("motionSmallNoise")->GetValueDouble(); + _sdf->GetElement("motionSmallNoise")->GetValue()->Get(motion_small_noise_); if (!_sdf->HasElement("motionDriftNoise")) motion_drift_noise_ = 0; else - motion_drift_noise_ = _sdf->GetElement("motionDriftNoise")->GetValueDouble(); + _sdf->GetElement("motionDriftNoise")->GetValue()->Get(motion_drift_noise_); if (!_sdf->HasElement("motionDriftNoiseTime")) motion_drift_noise_time_ = 1.0; else - motion_drift_noise_time_ = _sdf->GetElement("motionDriftNoiseTime")->GetValueDouble(); + _sdf->GetElement("motionDriftNoiseTime")->GetValue()->Get(motion_drift_noise_time_); controllers_.roll.Load(_sdf, "rollpitch"); @@ -405,11 +405,11 @@ void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, if (!_sdf) return; // _sdf->PrintDescription(_sdf->GetName()); - if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->GetValueDouble(); - if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->GetValueDouble(); - if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->GetValueDouble(); - if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->GetValueDouble(); - if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->GetValueDouble(); + if (_sdf->HasElement(prefix + "ProportionalGain")) _sdf->GetElement(prefix + "ProportionalGain")->GetValue()->Get(gain_p); + if (_sdf->HasElement(prefix + "DifferentialGain")) _sdf->GetElement(prefix + "DifferentialGain")->GetValue()->Get(gain_d); + if (_sdf->HasElement(prefix + "IntegralGain")) _sdf->GetElement(prefix + "IntegralGain")->GetValue()->Get(gain_i); + if (_sdf->HasElement(prefix + "TimeConstant")) _sdf->GetElement(prefix + "TimeConstant")->GetValue()->Get(time_constant); + if (_sdf->HasElement(prefix + "Limit")) _sdf->GetElement(prefix + "Limit")->GetValue()->Get(limit); } diff --git a/cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp b/cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp index 4137b16..f46dc3f 100644 --- a/cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp +++ b/cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp @@ -13,8 +13,8 @@ #include -#include "common/Events.hh" -#include "physics/physics.hh" +#include "gazebo/common/Events.hh" +#include "gazebo/physics/physics.hh" #include @@ -36,7 +36,7 @@ GazeboQuadrotorStateController::GazeboQuadrotorStateController() // Destructor GazeboQuadrotorStateController::~GazeboQuadrotorStateController() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); delete node_handle_; @@ -52,47 +52,47 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element if (!_sdf->HasElement("robotNamespace")) namespace_.clear(); else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString() + "/"; if (!_sdf->HasElement("topicName")) velocity_topic_ = "cmd_vel"; else - velocity_topic_ = _sdf->GetElement("topicName")->GetValueString(); + velocity_topic_ = _sdf->GetElement("topicName")->GetValue()->GetAsString(); if (!_sdf->HasElement("takeoffTopic")) takeoff_topic_ = "/ardrone/takeoff"; else - takeoff_topic_ = _sdf->GetElement("takeoffTopic")->GetValueString(); + takeoff_topic_ = _sdf->GetElement("takeoffTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("/ardrone/land")) land_topic_ = "/ardrone/land"; else - land_topic_ = _sdf->GetElement("landTopic")->GetValueString(); + land_topic_ = _sdf->GetElement("landTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("resetTopic")) reset_topic_ = "/ardrone/reset"; else - reset_topic_ = _sdf->GetElement("resetTopic")->GetValueString(); + reset_topic_ = _sdf->GetElement("resetTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("navdataTopic")) navdata_topic_ = "/ardrone/navdata"; else - navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValueString(); + navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("imuTopic")) imu_topic_.clear(); else - imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString(); + imu_topic_ = _sdf->GetElement("imuTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("sonarTopic")) sonar_topic_.clear(); else - sonar_topic_ = _sdf->GetElement("sonarTopic")->GetValueString(); + sonar_topic_ = _sdf->GetElement("sonarTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("stateTopic")) state_topic_.clear(); else - state_topic_ = _sdf->GetElement("stateTopic")->GetValueString(); + state_topic_ = _sdf->GetElement("stateTopic")->GetValue()->GetAsString(); if (!_sdf->HasElement("bodyName")) { @@ -100,8 +100,8 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element link_name_ = link->GetName(); } else { - link_name_ = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(link_name_)); + link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString(); + link = boost::dynamic_pointer_cast(world->GetEntity(link_name_)); } if (!link) diff --git a/cvg_sim_gazebo_plugins/src/reset_plugin.cpp b/cvg_sim_gazebo_plugins/src/reset_plugin.cpp index 95ebf9d..4ea0c0b 100644 --- a/cvg_sim_gazebo_plugins/src/reset_plugin.cpp +++ b/cvg_sim_gazebo_plugins/src/reset_plugin.cpp @@ -27,7 +27,7 @@ //================================================================================================= #include -#include "common/Events.hh" +#include "gazebo/common/Events.hh" #include diff --git a/cvg_sim_gazebo_plugins/urdf/quadrotor_sensors.urdf.xacro b/cvg_sim_gazebo_plugins/urdf/quadrotor_sensors.urdf.xacro index 6cecc97..ad539eb 100644 --- a/cvg_sim_gazebo_plugins/urdf/quadrotor_sensors.urdf.xacro +++ b/cvg_sim_gazebo_plugins/urdf/quadrotor_sensors.urdf.xacro @@ -21,11 +21,20 @@ xmlns:xacro="http://ros.org/wiki/xacro" /ardrone/imu 0 0 0 0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 + 0 0 0 0.5 0.5 0.5 + 0.0 0.0 0.0 0.35 0.35 0.3 + 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 0.00 0.00 0.00 0.0 + 0.0 + 0.0 0.00 @@ -35,8 +44,10 @@ xmlns:xacro="http://ros.org/wiki/xacro" base_link pressure_height altimeter - 0 + 0.0 + 0 0.1 + 0.0 0.5 @@ -45,8 +56,10 @@ xmlns:xacro="http://ros.org/wiki/xacro" 10.0 base_link magnetic - 0 0 0 + 0.0 0.0 0.0 + 0.0 0.0 0.0 0.0 0.0 0.0 + 0.0 0.0 0.0 1.3e-2 1.3e-2 1.3e-2 @@ -55,10 +68,15 @@ xmlns:xacro="http://ros.org/wiki/xacro" 4.0 base_link fix + 0.0 0.0 0.0 + 0.0 0.0 0.0 fix_velocity 5.0 5.0 5.0 + 0.0 0.0 0.0 0.1 0.1 0.1 0 0 0 + 0 0 0 + 0.0 0.0 0.0 0.1 0.1 0.1 diff --git a/readme.txt b/readme.txt deleted file mode 100644 index c66be49..0000000 --- a/readme.txt +++ /dev/null @@ -1,44 +0,0 @@ - - -These packages are used to simulate the flying robot Ardrone in ROS environment using gazebo simulator. Totally they are 4 packages. Their functions are descript as below: - -1. cvg_sim_gazebo: contains object models, sensor models, quadrocopter models, flying environment information and individual launch files for each objects and pure environment without any other objects. - -2. cvg_sim_gazebo_plugins: contains gazebo plugins for the quadrocopter model. quadrotor_simple_controller is used to control the robot motion and deliver navigation information, such as: /ardrone/navdata. Others are plugins for sensors in the quadrocopter, such as: IMU sensor, sonar sensor, GPS sensor. - -3. message_to_tf: is a package used to create a ros node, which transfers the ros topic /ground_truth/state to a /tf topic. - -4. cvg_sim_msgs: contains message forms for the simulator. - -Some packages are based on the tu-darmstadt-ros-pkg by Stefan Kohlbrecher, TU Darmstadt. - - -How to run a simulation: - -1. Download all the required packages, include four packages above and ardrone_autonomy package (which contains some standard message forms for ardrone simulator). - -2. Build packages cvg_sim_gazebo_plugins and message_to_tf. - rosmake cvg_sim_gazebo_plugins - rosmake message_to_tf - -3. Start a ros master by typing the following command in console - roscore - -4. Run a simulation by executing a launch file in cvg_test_sim package: - roslaunch cvg_sim_gazebo ardrone_testworld.launch - -5. You can manipulate the quadrocopter in gazebo simulator by downloading the ardrone_helpers package and using a joy-stick after calling this command: - roslaunch ardrone_joystick teleop.launch - - -Information for manipulation via joystick: - -1. The L1 button starts the quadrocopter. It also works as a deadman button so that the robot will land if you release it during flight. - -2. The left stick can be used to control the vx/vy-velocity. Keep in mind that these velocities are given in the local frame of the drone! - -3. The right stick controls the yaw-rate and the altitude. - -4. The select button can be used to switch between the two cameras. This can also be done by executing rosservice call /ardrone/togglecam. - -