From: Walter Fetter Lages Date: Sun, 18 Nov 2018 03:35:00 +0000 (-0200) Subject: Synchronize with Melodic gazebo_ros_control. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=c6e635aabd150d0c0fdc9124a7098c2af479982c;p=gazebo_ros_electrical.git Synchronize with Melodic gazebo_ros_control. --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b5a787..9a3f59c 100644 --- 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 --- 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 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 --- a/src/dcmotor_robot_hw_sim.cpp +++ b/src/dcmotor_robot_hw_sim.cpp @@ -48,7 +48,7 @@ #include - +//#include 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 tag in joint '" << joint_names_[j] << "'."); + } + + if(hardware_interface == "VoltageJointInterface") { + ROS_WARN_STREAM("Deprecated syntax, please prepend 'electrical_interface/' to '" << hardware_interface << "' within the 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