Synchronize with Melodic gazebo_ros_control.

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5b5a787..9a3f59c 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -113,8 +113,9 @@ find_package(gazebo REQUIRED)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   INCLUDE_DIRS include
-  LIBRARIES gazebo_ros_electrical
-#  CATKIN_DEPENDS angles control_toolbox gazebo hardware_interface joint_limits_interface pluginlib roscpp transmission_interface urdf electrical_interface
+  LIBRARIES ${PROJECT_NAME}
+  CATKIN_DEPENDS angles control_toolbox hardware_interface joint_limits_interface pluginlib roscpp transmission_interface urdf electrical_interface
+  # gazebo
 #  DEPENDS system_lib
 )
 
@@ -182,18 +183,18 @@ install(TARGETS ${PROJECT_NAME}
 )
 
 ## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+install(DIRECTORY include/${PROJECT_NAME}/
+   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
 #   FILES_MATCHING PATTERN "*.h"
 #   PATTERN ".svn" EXCLUDE
-# )
+)
 
 ## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
+install(FILES gazebo_ros_electrical_plugins.xml
 #   # myfile1
 #   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
+   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
 
 #############
 ## Testing ##

diff --git a/include/gazebo_ros_electrical/dcmotor_robot_hw_sim.h b/include/gazebo_ros_electrical/dcmotor_robot_hw_sim.h
index db38205..ef8a769 100644 (file)
--- a/include/gazebo_ros_electrical/dcmotor_robot_hw_sim.h
+++ b/include/gazebo_ros_electrical/dcmotor_robot_hw_sim.h
@@ -151,6 +151,8 @@ protected:
 
   std::vector<gazebo::physics::JointPtr> sim_joints_;
 
+  std::string physics_type_;
+
   // e_stop_active_ is true if the emergency stop is active.
   bool e_stop_active_, last_e_stop_active_;
 };

diff --git a/src/dcmotor_robot_hw_sim.cpp b/src/dcmotor_robot_hw_sim.cpp
index caa7b5c..e5ea0b6 100644 (file)
--- a/src/dcmotor_robot_hw_sim.cpp
+++ b/src/dcmotor_robot_hw_sim.cpp
@@ -48,7 +48,7 @@
 
 
 #include <gazebo_ros_electrical/dcmotor_robot_hw_sim.h>
-
+//#include <udf/model.h>
 
 namespace
 {
@@ -167,7 +167,7 @@ bool DCmotorRobotHWSim::initSim(
 
     // Decide what kind of command interface this actuator/joint has
     hardware_interface::JointHandle joint_handle;
-    if(hardware_interface == "EffortJointInterface")
+    if(hardware_interface == "EffortJointInterface" || hardware_interface == "hardware_interface/EffortJointInterface")
     {
       // Create effort joint interface
       joint_control_methods_[j] = EFFORT;
@@ -175,7 +175,7 @@ bool DCmotorRobotHWSim::initSim(
                                                      &joint_effort_command_[j]);
       ej_interface_.registerHandle(joint_handle);
     }
-    else if(hardware_interface == "PositionJointInterface")
+    else if(hardware_interface == "PositionJointInterface" || hardware_interface == "hardware_interface/PositionJointInterface")
     {
       // Create position joint interface
       joint_control_methods_[j] = POSITION;
@@ -183,7 +183,7 @@ bool DCmotorRobotHWSim::initSim(
                                                      &joint_position_command_[j]);
       pj_interface_.registerHandle(joint_handle);
     }
-    else if(hardware_interface == "VelocityJointInterface")
+    else if(hardware_interface == "VelocityJointInterface" || hardware_interface == "hardware_interface/VelocityJointInterface")
     {
       // Create velocity joint interface
       joint_control_methods_[j] = VELOCITY;
@@ -191,7 +191,7 @@ bool DCmotorRobotHWSim::initSim(
                                                      &joint_velocity_command_[j]);
       vj_interface_.registerHandle(joint_handle);
     }
-    else if(hardware_interface == "VoltageJointInterface")
+    else if(hardware_interface == "VoltageJointInterface" || hardware_interface == "electrical_interface/VoltageJointInterface")
     {
       // Create voltage joint interface
       joint_control_methods_[j] = VOLTAGE;
@@ -202,22 +202,42 @@ bool DCmotorRobotHWSim::initSim(
     else
     {
       ROS_FATAL_STREAM_NAMED("dcmotor_robot_hw_sim","No matching hardware interface found for '"
-        << hardware_interface );
+        << hardware_interface << "' while loading interfaces for " << joint_names_[j]);
       return false;
     }
 
+    if(hardware_interface == "EffortJointInterface" || hardware_interface == "PositionJointInterface" || hardware_interface == "VelocityJointInterface") {
+      ROS_WARN_STREAM("Deprecated syntax, please prepend 'hardware_interface/' to '" << hardware_interface << "' within the <hardwareInterface> tag in joint '" << joint_names_[j] << "'.");
+    }
+    
+    if(hardware_interface == "VoltageJointInterface") {
+      ROS_WARN_STREAM("Deprecated syntax, please prepend 'electrical_interface/' to '" << hardware_interface << "' within the <hardwareInterface> tag in joint '" << joint_names_[j] << "'.");
+    }
+
     // Get the gazebo joint that corresponds to the robot joint.
     //ROS_DEBUG_STREAM_NAMED("dcmotor_robot_hw_sim", "Getting pointer to gazebo joint: "
     //  << joint_names_[j]);
     gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
     if (!joint)
     {
-      ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
+      ROS_ERROR_STREAM_NAMED("dcmotor_robot_hw_sim","This robot has a joint named \"" << joint_names_[j]
         << "\" which is not in the gazebo model.");
       return false;
     }
     sim_joints_.push_back(joint);
 
+    // get physics engine type
+#if GAZEBO_MAJOR_VERSION >= 8
+    gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->Physics();
+#else
+    gazebo::physics::PhysicsEnginePtr physics = gazebo::physics::get_world()->GetPhysicsEngine();
+#endif
+    physics_type_ = physics->GetType();
+    if (physics_type_.empty())
+    {
+      ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "No physics type found.");
+    }
+
     registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
                         joint_limit_nh, urdf_model,
                         &joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
@@ -226,9 +246,9 @@ bool DCmotorRobotHWSim::initSim(
     {
       // Initialize the PID controller. If no PID gain values are found, use joint->SetAngle() or
       // joint->SetParam("vel") to control the joint.
-      const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" +
+      const ros::NodeHandle nh(robot_namespace + "/gazebo_ros_control/pid_gains/" +
                                joint_names_[j]);
-      if (pid_controllers_[j].init(nh, true))
+      if (pid_controllers_[j].init(nh))
       {
         switch (joint_control_methods_[j])
         {
@@ -341,10 +361,10 @@ void DCmotorRobotHWSim::writeSim(ros::Time time, ros::Duration period)
         break;
 
       case POSITION:
-#if GAZEBO_MAJOR_VERSION >= 4
-        sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
+#if GAZEBO_MAJOR_VERSION >= 9
+        sim_joints_[j]->SetPosition(0, joint_position_command_[j], true);
 #else
-        sim_joints_[j]->SetAngle(0, joint_position_command_[j]);
+        sim_joints_[j]->SetPosition(0, joint_position_command_[j]);
 #endif
         break;
 
@@ -377,7 +397,14 @@ void DCmotorRobotHWSim::writeSim(ros::Time time, ros::Duration period)
 
       case VELOCITY:
 #if GAZEBO_MAJOR_VERSION > 2
-        sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
+        if (physics_type_.compare("dart") == 0)
+        {
+          sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
+        }
+        else 
+        {
+          sim_joints_[j]->SetParam("vel", 0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
+        }
 #else
         sim_joints_[j]->SetVelocity(0, e_stop_active_ ? 0 : joint_velocity_command_[j]);
 #endif