From ea1e44b6de48846a09c3dcb0482daf73a0e45769 Mon Sep 17 00:00:00 2001
From: hshen11 <hshen11@jhu.edu>
Date: Thu, 14 May 2020 20:46:08 -0400
Subject: [PATCH 1/2] change we control

---
 .../quadrotor_hardware_gazebo.h                               | 4 +++-
 .../src/quadrotor_hardware_gazebo.cpp                         | 3 +++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller_gazebo/quadrotor_hardware_gazebo.h b/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller_gazebo/quadrotor_hardware_gazebo.h
index 50b35317..d113f356 100644
--- a/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller_gazebo/quadrotor_hardware_gazebo.h
+++ b/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller_gazebo/quadrotor_hardware_gazebo.h
@@ -41,12 +41,14 @@
 #include <hector_uav_msgs/MotorStatus.h>
 #include <hector_uav_msgs/EnableMotors.h>
 
+#include <gazebo_ros_control/default_robot_hw_sim.h>
+
 namespace hector_quadrotor_controller_gazebo
 {
 
 using namespace hector_quadrotor_interface;
 
-class QuadrotorHardwareSim : public gazebo_ros_control::RobotHWSim
+class QuadrotorHardwareSim : public gazebo_ros_control::DefaultRobotHWSim
 {
 public:
   QuadrotorHardwareSim();
diff --git a/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp b/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
index 451a0c30..3344eda2 100644
--- a/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
+++ b/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
@@ -59,6 +59,7 @@ bool QuadrotorHardwareSim::initSim(
     std::vector<transmission_interface::TransmissionInfo> transmissions)
 {
   // store parent model pointer
+  gazebo_ros_control::DefaultRobotHWSim::initSim(robot_namespace,model_nh,parent_model,urdf_model,transmissions);
   model_ = parent_model;
   link_ = model_->GetLink();
 #if (GAZEBO_MAJOR_VERSION >= 8)
@@ -118,6 +119,7 @@ bool QuadrotorHardwareSim::initSim(
 void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
 {
   // read state from Gazebo
+  gazebo_ros_control::DefaultRobotHWSim::readSim(time,period);
   const double acceleration_time_constant = 0.1;
 #if (GAZEBO_MAJOR_VERSION >= 8)
   gz_acceleration_ = ((link_->WorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) /
@@ -232,6 +234,7 @@ void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
 
 void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period)
 {
+  gazebo_ros_control::DefaultRobotHWSim::writeSim(time,period);
   bool result_written = false;
 
   if (motor_output_->connected() && motor_output_->enabled()) {

From 5c55d3ef095b4e9ca07996f3e46f2bbbfbdccc1f Mon Sep 17 00:00:00 2001
From: hshen11 <hshen11@jhu.edu>
Date: Fri, 15 May 2020 11:20:23 -0400
Subject: [PATCH 2/2] change the code format

---
 .../src/quadrotor_hardware_gazebo.cpp                     | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp b/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
index 3344eda2..bb468e05 100644
--- a/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
+++ b/hector_quadrotor_controller_gazebo/src/quadrotor_hardware_gazebo.cpp
@@ -58,8 +58,9 @@ bool QuadrotorHardwareSim::initSim(
     const urdf::Model *const urdf_model,
     std::vector<transmission_interface::TransmissionInfo> transmissions)
 {
+  // initialize default sim interface
+  gazebo_ros_control::DefaultRobotHWSim::initSim(robot_namespace, model_nh, parent_model, urdf_model, transmissions);
   // store parent model pointer
-  gazebo_ros_control::DefaultRobotHWSim::initSim(robot_namespace,model_nh,parent_model,urdf_model,transmissions);
   model_ = parent_model;
   link_ = model_->GetLink();
 #if (GAZEBO_MAJOR_VERSION >= 8)
@@ -119,7 +120,7 @@ bool QuadrotorHardwareSim::initSim(
 void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
 {
   // read state from Gazebo
-  gazebo_ros_control::DefaultRobotHWSim::readSim(time,period);
+  gazebo_ros_control::DefaultRobotHWSim::readSim(time, period);
   const double acceleration_time_constant = 0.1;
 #if (GAZEBO_MAJOR_VERSION >= 8)
   gz_acceleration_ = ((link_->WorldLinearVel() - gz_velocity_) + acceleration_time_constant * gz_acceleration_) /
@@ -234,7 +235,8 @@ void QuadrotorHardwareSim::readSim(ros::Time time, ros::Duration period)
 
 void QuadrotorHardwareSim::writeSim(ros::Time time, ros::Duration period)
 {
-  gazebo_ros_control::DefaultRobotHWSim::writeSim(time,period);
+
+  gazebo_ros_control::DefaultRobotHWSim::writeSim(time, period);
   bool result_written = false;
 
   if (motor_output_->connected() && motor_output_->enabled()) {