diff --git a/README.md b/README.md index 8440554..0db8ae7 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,49 @@ -tum_simulator ported to Hydro +tum_simulator on Indigo ============= + +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 install the simulator: + +1. Create a workspace for the simulator + + ``` + mkdir -p ~/tum_simulator_ws/src + cd ~/tum_simulator_ws/src + catkin_init_workspace + ``` +2. Download dependencies + + ``` + git clone https://github.com/AutonomyLab/ardrone_autonomy.git # The AR.Drone ROS driver + git clone https://github.com/occomco/tum_simulator.git + cd .. + rosdep install --from-paths src --ignore-src --rosdistro indigo -y + ``` +3. Build the simulator + + ``` + catkin_make + ``` +4. Source the environment + + ``` + source devel/setup.bash + ``` +How to run a simulation: + +1. Run a simulation by executing a launch file in cvg_sim_gazebo package: + + ``` + roslaunch cvg_sim_gazebo ardrone_testworld.launch + ``` diff --git a/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro index 158639a..6f0c637 100644 --- a/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro +++ b/cvg_sim_gazebo/urdf/quadrotor.urdf.xacro @@ -7,7 +7,7 @@ xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"> - + diff --git a/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro index bdc5c23..692fdb2 100644 --- a/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro +++ b/cvg_sim_gazebo/urdf/quadrotor_base.urdf.xacro @@ -6,7 +6,7 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:xacro="http://ros.org/wiki/xacro" > - + diff --git a/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro b/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro index 99e7bbd..f2465a5 100644 --- a/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro +++ b/cvg_sim_gazebo/urdf/quadrotor_sensors.urdf.xacro @@ -9,36 +9,36 @@ xmlns:xacro="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"> - + - + - + - + - + diff --git a/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro b/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro index 3e88c19..8a02b7f 100644 --- a/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro +++ b/cvg_sim_gazebo/urdf/sensors/generic_camera.urdf.xacro @@ -93,7 +93,7 @@ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" true - PR2/Blue + Gazebo/Blue 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 index 31fc3a7..e43f09d 100644 --- 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 @@ -27,13 +27,13 @@ #include -#include "common/Plugin.hh" -#include "common/Time.hh" -//#include "physics/physics.h" +#include "gazebo/common/Plugin.hh" +#include "gazebo/common/Time.hh" +//#include "gazebo/physics/physics.h" //#include "transport/TransportTypes.hh" //#include "msgs/MessageTypes.hh" -//#include "common/Time.hh" -//#include "common/Events.hh" +//#include "gazebo/common/Time.hh" +//#include "gazebo/common/Events.hh" // ROS #include 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 index 3c70110..b020d76 100644 --- 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 @@ -29,8 +29,8 @@ #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 "gazebo/common/Plugin.hh" +#include "gazebo/common/Time.hh" #include #include 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 index 351ac9f..c3505e4 100644 --- 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 @@ -64,8 +64,8 @@ #include #endif -#include "common/Plugin.hh" -#include "common/Time.hh" +#include "gazebo/common/Plugin.hh" +#include "gazebo/common/Time.hh" #include #include 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 index f24071f..26cfe9b 100644 --- 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 @@ -29,8 +29,8 @@ #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 "gazebo/common/Plugin.hh" +#include "gazebo/common/Time.hh" #include #include 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 index 9c96c1f..8afcf74 100644 --- 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 @@ -29,8 +29,8 @@ #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 "gazebo/common/Plugin.hh" +#include "gazebo/common/Time.hh" #include #include 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..778740c 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 = _offset->Get(); + if (_drift && !_drift->GetValue()->Get(drift)) drift = _drift->Get(); + if (_drift_frequency && !_drift_frequency->GetValue()->Get(drift_frequency)) drift_frequency = _drift_frequency->Get(); + if (_gaussian_noise && !_gaussian_noise->GetValue()->Get(gaussian_noise)) gaussian_noise = _gaussian_noise->Get(); } 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..feb70ff 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 @@ -140,6 +140,14 @@ class GazeboQuadrotorStateController : public ModelPlugin std::string sonar_topic_; std::string state_topic_; + std::string cam_out_topic_; + std::string cam_front_in_topic_; + std::string cam_bottom_in_topic_; + + std::string cam_info_out_topic_; + std::string cam_info_front_in_topic_; + std::string cam_info_bottom_in_topic_; + // extra parameters for robot control. bool m_isFlying; bool m_takeoff; 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 index 53a68ff..6e52236 100644 --- a/cvg_sim_gazebo_plugins/src/diffdrive_plugin_6w.cpp +++ b/cvg_sim_gazebo_plugins/src/diffdrive_plugin_6w.cpp @@ -34,8 +34,8 @@ #include #include -#include "common/Events.hh" -#include "physics/physics.hh" +#include "gazebo/common/Events.hh" +#include "gazebo/physics/physics.hh" #include #include @@ -65,7 +65,7 @@ DiffDrivePlugin6W::DiffDrivePlugin6W() // Destructor DiffDrivePlugin6W::~DiffDrivePlugin6W() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); delete transform_broadcaster_; rosnode_->shutdown(); callback_queue_thread_.join(); @@ -81,12 +81,12 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("robotNamespace")) robotNamespace.clear(); else - robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + robotNamespace = _sdf->GetElement("robotNamespace")->Get() + "/"; if (!_sdf->HasElement("topicName")) topicName = "cmd_vel"; else - topicName = _sdf->GetElement("topicName")->GetValueString(); + topicName = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("bodyName")) { @@ -94,8 +94,8 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) linkName = link->GetName(); } else { - linkName = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(_sdf->GetElement("bodyName")->GetValueString())); + linkName = _sdf->GetElement("bodyName")->Get(); + link = boost::dynamic_pointer_cast(world->GetEntity(_sdf->GetElement("bodyName")->Get())); } // assert that the body by linkName exists @@ -105,12 +105,12 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 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 (_sdf->HasElement("frontLeftJoint")) joints[FRONT_LEFT] = _model->GetJoint(_sdf->GetElement("frontLeftJoint")->Get()); + if (_sdf->HasElement("frontRightJoint")) joints[FRONT_RIGHT] = _model->GetJoint(_sdf->GetElement("frontRightJoint")->Get()); + if (_sdf->HasElement("midLeftJoint")) joints[MID_LEFT] = _model->GetJoint(_sdf->GetElement("midLeftJoint")->Get()); + if (_sdf->HasElement("midRightJoint")) joints[MID_RIGHT] = _model->GetJoint(_sdf->GetElement("midRightJoint")->Get()); + if (_sdf->HasElement("rearLeftJoint")) joints[REAR_LEFT] = _model->GetJoint(_sdf->GetElement("rearLeftJoint")->Get()); + if (_sdf->HasElement("rearRightJoint")) joints[REAR_RIGHT] = _model->GetJoint(_sdf->GetElement("rearRightJoint")->Get()); 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"); @@ -122,17 +122,17 @@ void DiffDrivePlugin6W::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("wheelSeparation")) wheelSep = 0.34; else - wheelSep = _sdf->GetElement("wheelSeparation")->GetValueDouble(); + wheelSep = _sdf->GetElement("wheelSeparation")->Get(); if (!_sdf->HasElement("wheelDiameter")) wheelDiam = 0.15; else - wheelDiam = _sdf->GetElement("wheelDiameter")->GetValueDouble(); + wheelDiam = _sdf->GetElement("wheelDiameter")->Get(); if (!_sdf->HasElement("torque")) torque = 10.0; else - torque = _sdf->GetElement("torque")->GetValueDouble(); + torque = _sdf->GetElement("torque")->Get(); // start ros node if (!ros::isInitialized()) diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_baro.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_baro.cpp index 3876a5b..5c72377 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")->Get() + "/"; 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")->Get(); + 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")) update_rate = _sdf->GetElement("updateRate")->Get(); 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")->Get(); if (!_sdf->HasElement("topicName")) height_topic_ = "pressure_height"; else - height_topic_ = _sdf->GetElement("topicName")->GetValueString(); + height_topic_ = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("altimeterTopicName")) altimeter_topic_ = "altimeter"; else - altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->GetValueString(); + altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->Get(); if (!_sdf->HasElement("elevation")) elevation_ = DEFAULT_ELEVATION; else - elevation_ = _sdf->GetElement("elevation")->GetValueDouble(); + elevation_ = _sdf->GetElement("elevation")->Get(); if (!_sdf->HasElement("qnh")) qnh_ = DEFAULT_QNH; else - qnh_ = _sdf->GetElement("qnh")->GetValueDouble(); + qnh_ = _sdf->GetElement("qnh")->Get(); 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 index a13f18d..4b58783 100644 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_gps.cpp +++ b/cvg_sim_gazebo_plugins/src/gazebo_ros_gps.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 EARTH_RADIUS = 6371000.0; static const double DEFAULT_REFERENCE_LATITUDE = 49.9; @@ -46,7 +46,7 @@ GazeboRosGps::GazeboRosGps() // Destructor GazeboRosGps::~GazeboRosGps() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); delete node_handle_; } @@ -61,7 +61,7 @@ void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("robotNamespace")) namespace_.clear(); else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; if (!_sdf->HasElement("bodyName")) { @@ -69,8 +69,8 @@ void GazeboRosGps::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")->Get(); + link = boost::dynamic_pointer_cast(world->GetEntity(link_name_)); } if (!link) @@ -80,53 +80,53 @@ void GazeboRosGps::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) } double update_rate = 4.0; - if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->GetValueDouble(); + if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->Get(); 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(); + frame_id_ = _sdf->GetElement("frameId")->Get(); if (!_sdf->HasElement("topicName")) fix_topic_ = "fix"; else - fix_topic_ = _sdf->GetElement("topicName")->GetValueString(); + fix_topic_ = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("velocityTopicName")) velocity_topic_ = "fix_velocity"; else - velocity_topic_ = _sdf->GetElement("velocityTopicName")->GetValueString(); + velocity_topic_ = _sdf->GetElement("velocityTopicName")->Get(); if (!_sdf->HasElement("referenceLatitude")) reference_latitude_ = DEFAULT_REFERENCE_LATITUDE; else - reference_latitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble(); + reference_latitude_ = _sdf->GetElement("referenceLatitude")->Get(); if (!_sdf->HasElement("referenceLongitude")) reference_longitude_ = DEFAULT_REFERENCE_LONGITUDE; else - reference_longitude_ = _sdf->GetElement("referenceLongitude")->GetValueDouble(); + reference_longitude_ = _sdf->GetElement("referenceLongitude")->Get(); if (!_sdf->HasElement("referenceHeading")) reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0; else - reference_heading_ = _sdf->GetElement("referenceLatitude")->GetValueDouble() * M_PI/180.0; + reference_heading_ = _sdf->GetElement("referenceLatitude")->Get() * M_PI/180.0; if (!_sdf->HasElement("referenceAltitude")) reference_altitude_ = DEFAULT_REFERENCE_ALTITUDE; else - reference_altitude_ = _sdf->GetElement("referenceLatitude")->GetValueDouble(); + reference_altitude_ = _sdf->GetElement("referenceLatitude")->Get(); if (!_sdf->HasElement("status")) status_ = sensor_msgs::NavSatStatus::STATUS_FIX; else - status_ = _sdf->GetElement("status")->GetValueUInt(); + status_ = _sdf->GetElement("status")->Get(); if (!_sdf->HasElement("service")) service_ = sensor_msgs::NavSatStatus::SERVICE_GPS; else - service_ = _sdf->GetElement("service")->GetValueUInt(); + service_ = _sdf->GetElement("service")->Get(); fix_.header.frame_id = frame_id_; fix_.status.status = status_; diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp index 4eb1ccf..355b354 100644 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp +++ b/cvg_sim_gazebo_plugins/src/gazebo_ros_imu.cpp @@ -55,8 +55,8 @@ //================================================================================================= #include -#include "common/Events.hh" -#include "physics/physics.hh" +#include "gazebo/common/Events.hh" +#include "gazebo/physics/physics.hh" namespace gazebo { @@ -77,7 +77,7 @@ GazeboRosIMU::GazeboRosIMU() // Destructor GazeboRosIMU::~GazeboRosIMU() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); #ifdef USE_CBQ @@ -97,7 +97,7 @@ void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("robotNamespace")) robotNamespace.clear(); else - robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + robotNamespace = _sdf->GetElement("robotNamespace")->Get() + "/"; if (!_sdf->HasElement("bodyName")) { @@ -105,8 +105,8 @@ void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) linkName = link->GetName(); } else { - linkName = _sdf->GetElement("bodyName")->GetValueString(); - link = boost::shared_dynamic_cast(world->GetEntity(linkName)); + linkName = _sdf->GetElement("bodyName")->Get(); + link = boost::dynamic_pointer_cast(world->GetEntity(linkName)); } // assert that the body by linkName exists @@ -117,23 +117,23 @@ void GazeboRosIMU::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")) update_rate = _sdf->GetElement("updateRate")->Get(); update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0; if (!_sdf->HasElement("frameId")) frameId = linkName; else - frameId = _sdf->GetElement("frameId")->GetValueString(); + frameId = _sdf->GetElement("frameId")->Get(); if (!_sdf->HasElement("topicName")) topicName = "imu"; else - topicName = _sdf->GetElement("topicName")->GetValueString(); + topicName = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("serviceName")) serviceName = topicName + "/calibrate"; else - serviceName = _sdf->GetElement("serviceName")->GetValueString(); + serviceName = _sdf->GetElement("serviceName")->Get(); accelModel.Load(_sdf, "accel"); rateModel.Load(_sdf, "rate"); @@ -141,7 +141,7 @@ void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // also use old configuration variables from gazebo_ros_imu if (_sdf->HasElement("gaussianNoise")) { - double gaussianNoise = _sdf->GetElement("gaussianNoise")->GetValueDouble(); + double gaussianNoise = _sdf->GetElement("gaussianNoise")->Get(); if (gaussianNoise != 0.0) { accelModel.gaussian_noise = gaussianNoise; rateModel.gaussian_noise = gaussianNoise; @@ -149,7 +149,7 @@ void GazeboRosIMU::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) } if (_sdf->HasElement("rpyOffset")) { - sdf::Vector3 sdfVec = _sdf->GetElement("rpyOffset")->GetValueVector3(); + sdf::Vector3 sdfVec = _sdf->GetElement("rpyOffset")->Get(); 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; diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp index da62df4..1cdce53 100644 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.cpp +++ b/cvg_sim_gazebo_plugins/src/gazebo_ros_magnetic.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_MAGNITUDE = 1.0; static const double DEFAULT_REFERENCE_HEADING = 0.0; @@ -45,7 +45,7 @@ GazeboRosMagnetic::GazeboRosMagnetic() // Destructor GazeboRosMagnetic::~GazeboRosMagnetic() { - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); delete node_handle_; @@ -61,12 +61,12 @@ void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (!_sdf->HasElement("robotNamespace")) namespace_.clear(); else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; if (!_sdf->HasElement("topicName")) topic_ = "magnetic"; else - topic_ = _sdf->GetElement("topicName")->GetValueString(); + topic_ = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("bodyName")) { @@ -74,8 +74,8 @@ void GazeboRosMagnetic::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")->Get(); + link = boost::dynamic_pointer_cast(world->GetEntity(link_name_)); } if (!link) @@ -85,33 +85,33 @@ void GazeboRosMagnetic::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")) update_rate = _sdf->GetElement("updateRate")->Get(); 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(); + frame_id_ = _sdf->GetElement("frameId")->Get(); if (!_sdf->HasElement("magnitude")) magnitude_ = DEFAULT_MAGNITUDE; else - magnitude_ = _sdf->GetElement("magnitude")->GetValueDouble(); + magnitude_ = _sdf->GetElement("magnitude")->Get(); if (!_sdf->HasElement("referenceHeading")) reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0; else - reference_heading_ = _sdf->GetElement("referenceHeading")->GetValueDouble() * M_PI/180.0; + reference_heading_ = _sdf->GetElement("referenceHeading")->Get() * 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; + declination_ = _sdf->GetElement("declination")->Get() * 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; + inclination_ = _sdf->GetElement("inclination")->Get() * M_PI/180.0; // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings magnetic_field_.header.frame_id = frame_id_; diff --git a/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp b/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp index fd6bd6c..dde56d0 100644 --- a/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp +++ b/cvg_sim_gazebo_plugins/src/gazebo_ros_sonar.cpp @@ -27,9 +27,9 @@ //================================================================================================= #include -#include "common/Events.hh" -#include "physics/physics.hh" -#include "sensors/RaySensor.hh" +#include "gazebo/common/Events.hh" +#include "gazebo/physics/physics.hh" +#include "gazebo/sensors/RaySensor.hh" #include @@ -44,7 +44,7 @@ GazeboRosSonar::GazeboRosSonar() GazeboRosSonar::~GazeboRosSonar() { sensor_->SetActive(false); - event::Events::DisconnectWorldUpdateStart(updateConnection); + event::Events::DisconnectWorldUpdateBegin(updateConnection); node_handle_->shutdown(); delete node_handle_; } @@ -54,7 +54,7 @@ GazeboRosSonar::~GazeboRosSonar() void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) { // Get then name of the parent sensor - sensor_ = boost::shared_dynamic_cast(_sensor); + sensor_ = boost::dynamic_pointer_cast(_sensor); if (!sensor_) { gzthrow("GazeboRosSonar requires a Ray Sensor as its parent"); @@ -69,17 +69,17 @@ void GazeboRosSonar::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf) if (!_sdf->HasElement("robotNamespace")) namespace_.clear(); else - namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/"; + namespace_ = _sdf->GetElement("robotNamespace")->Get() + "/"; if (!_sdf->HasElement("frameId")) frame_id_ = ""; else - frame_id_ = _sdf->GetElement("frameId")->GetValueString(); + frame_id_ = _sdf->GetElement("frameId")->Get(); if (!_sdf->HasElement("topicName")) topic_ = "sonar"; else - topic_ = _sdf->GetElement("topicName")->GetValueString(); + topic_ = _sdf->GetElement("topicName")->Get(); sensor_model_.Load(_sdf); diff --git a/cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp b/cvg_sim_gazebo_plugins/src/quadrotor_simple_controller.cpp index 5e0dc65..92360e1 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")->Get() + "/"; if (!_sdf->HasElement("topicName")) velocity_topic_ = "cmd_vel"; else - velocity_topic_ = _sdf->GetElement("topicName")->GetValueString(); + velocity_topic_ = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("navdataTopic")) navdata_topic_ = "/ardrone/navdata"; else - navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValueString(); + navdata_topic_ = _sdf->GetElement("navdataTopic")->Get(); if (!_sdf->HasElement("imuTopic")) imu_topic_.clear(); else - imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString(); + imu_topic_ = _sdf->GetElement("imuTopic")->Get(); if (!_sdf->HasElement("stateTopic")) state_topic_.clear(); else - state_topic_ = _sdf->GetElement("stateTopic")->GetValueString(); + state_topic_ = _sdf->GetElement("stateTopic")->Get(); 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")->Get(); + 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(); + max_force_ = _sdf->GetElement("maxForce")->Get(); if (!_sdf->HasElement("motionSmallNoise")) motion_small_noise_ = 0; else - motion_small_noise_ = _sdf->GetElement("motionSmallNoise")->GetValueDouble(); + motion_small_noise_ = _sdf->GetElement("motionSmallNoise")->Get(); if (!_sdf->HasElement("motionDriftNoise")) motion_drift_noise_ = 0; else - motion_drift_noise_ = _sdf->GetElement("motionDriftNoise")->GetValueDouble(); + motion_drift_noise_ = _sdf->GetElement("motionDriftNoise")->Get(); if (!_sdf->HasElement("motionDriftNoiseTime")) motion_drift_noise_time_ = 1.0; else - motion_drift_noise_time_ = _sdf->GetElement("motionDriftNoiseTime")->GetValueDouble(); + motion_drift_noise_time_ = _sdf->GetElement("motionDriftNoiseTime")->Get(); 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")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->Get(); + if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->Get(); + if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->Get(); + if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->Get(); + if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->Get(); } diff --git a/cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp b/cvg_sim_gazebo_plugins/src/quadrotor_state_controller.cpp index 4137b16..2865cf0 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")->Get() + "/"; if (!_sdf->HasElement("topicName")) velocity_topic_ = "cmd_vel"; else - velocity_topic_ = _sdf->GetElement("topicName")->GetValueString(); + velocity_topic_ = _sdf->GetElement("topicName")->Get(); if (!_sdf->HasElement("takeoffTopic")) takeoff_topic_ = "/ardrone/takeoff"; else - takeoff_topic_ = _sdf->GetElement("takeoffTopic")->GetValueString(); + takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get(); - if (!_sdf->HasElement("/ardrone/land")) + if (!_sdf->HasElement("landTopic")) land_topic_ = "/ardrone/land"; else - land_topic_ = _sdf->GetElement("landTopic")->GetValueString(); + land_topic_ = _sdf->GetElement("landTopic")->Get(); if (!_sdf->HasElement("resetTopic")) reset_topic_ = "/ardrone/reset"; else - reset_topic_ = _sdf->GetElement("resetTopic")->GetValueString(); + reset_topic_ = _sdf->GetElement("resetTopic")->Get(); if (!_sdf->HasElement("navdataTopic")) navdata_topic_ = "/ardrone/navdata"; else - navdata_topic_ = _sdf->GetElement("navdataTopic")->GetValueString(); + navdata_topic_ = _sdf->GetElement("navdataTopic")->Get(); if (!_sdf->HasElement("imuTopic")) imu_topic_.clear(); else - imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString(); + imu_topic_ = _sdf->GetElement("imuTopic")->Get(); if (!_sdf->HasElement("sonarTopic")) sonar_topic_.clear(); else - sonar_topic_ = _sdf->GetElement("sonarTopic")->GetValueString(); + sonar_topic_ = _sdf->GetElement("sonarTopic")->Get(); if (!_sdf->HasElement("stateTopic")) state_topic_.clear(); else - state_topic_ = _sdf->GetElement("stateTopic")->GetValueString(); + state_topic_ = _sdf->GetElement("stateTopic")->Get(); if (!_sdf->HasElement("bodyName")) { @@ -100,10 +100,40 @@ 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")->Get(); + link = boost::dynamic_pointer_cast(world->GetEntity(link_name_)); } + if (!_sdf->HasElement("camOutTopic")) + cam_out_topic_ = "/ardrone/image_raw"; + else + cam_out_topic_ = _sdf->GetElement("camOutTopic")->Get(); + + if (!_sdf->HasElement("camFrontTopic")) + cam_front_in_topic_ = "/ardrone/front/image_raw"; + else + cam_front_in_topic_ = _sdf->GetElement("camFrontTopic")->Get(); + + if (!_sdf->HasElement("camBottomTopic")) + cam_bottom_in_topic_ = "/ardrone/bottom/image_raw"; + else + cam_bottom_in_topic_ = _sdf->GetElement("camBottomTopic")->Get(); + + if (!_sdf->HasElement("camInfoOutTopic")) + cam_info_out_topic_ = "/ardrone/camera_info"; + else + cam_info_out_topic_ = _sdf->GetElement("camInfoOutTopic")->Get(); + + if (!_sdf->HasElement("camInfoFrontTopic")) + cam_info_front_in_topic_ = "/ardrone/front/camera_info"; + else + cam_info_front_in_topic_ = _sdf->GetElement("camInfoFrontTopic")->Get(); + + if (!_sdf->HasElement("camInfoBottomTopic")) + cam_info_bottom_in_topic_ = "/ardrone/bottom/camera_info"; + else + cam_info_bottom_in_topic_ = _sdf->GetElement("camInfoBottomTopic")->Get(); + if (!link) { ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str()); @@ -204,39 +234,33 @@ void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::Element toggleCam_service = node_handle_->advertiseService(toggleCam_ops); // camera image data - std::string cam_out_topic = "/ardrone/image_raw"; - std::string cam_front_in_topic = "/ardrone/front/image_raw"; - std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw"; std::string in_transport = "raw"; camera_it_ = new image_transport::ImageTransport(*node_handle_); - camera_publisher_ = camera_it_->advertise(cam_out_topic, 1); + camera_publisher_ = camera_it_->advertise(cam_out_topic_, 1); camera_front_subscriber_ = camera_it_->subscribe( - cam_front_in_topic, 1, + cam_front_in_topic_, 1, boost::bind(&GazeboQuadrotorStateController::CameraFrontCallback, this, _1), ros::VoidPtr(), in_transport); camera_bottom_subscriber_ = camera_it_->subscribe( - cam_bottom_in_topic, 1, + cam_bottom_in_topic_, 1, boost::bind(&GazeboQuadrotorStateController::CameraBottomCallback, this, _1), ros::VoidPtr(), in_transport); // camera image data - std::string cam_info_out_topic = "/ardrone/camera_info"; - std::string cam_info_front_in_topic = "/ardrone/front/camera_info"; - std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info"; - camera_info_publisher_ = node_handle_->advertise(cam_info_out_topic,1); + camera_info_publisher_ = node_handle_->advertise(cam_info_out_topic_,1); ros::SubscribeOptions cam_info_front_ops = ros::SubscribeOptions::create( - cam_info_front_in_topic, 1, + cam_info_front_in_topic_, 1, boost::bind(&GazeboQuadrotorStateController::CameraInfoFrontCallback, this, _1), ros::VoidPtr(), &callback_queue_); camera_info_front_subscriber_ = node_handle_->subscribe(cam_info_front_ops); ros::SubscribeOptions cam_info_bottom_ops = ros::SubscribeOptions::create( - cam_info_bottom_in_topic, 1, + cam_info_bottom_in_topic_, 1, boost::bind(&GazeboQuadrotorStateController::CameraInfoBottomCallback, this, _1), ros::VoidPtr(), &callback_queue_); camera_info_bottom_subscriber_ = node_handle_->subscribe(cam_info_bottom_ops); 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/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. - -