From: Walter Fetter Lages Date: Fri, 8 Oct 2021 04:13:08 +0000 (-0300) Subject: Port to Galactic. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;p=roboticsgroup_upatras_gazebo_plugins.git Port to Galactic. --- diff --git a/CMakeLists.txt b/CMakeLists.txt index 1b69e4a..0511a27 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,51 +1,77 @@ -cmake_minimum_required(VERSION 3.1.0) +cmake_minimum_required(VERSION 3.8) project(roboticsgroup_upatras_gazebo_plugins) -find_package(catkin REQUIRED - COMPONENTS - control_toolbox - gazebo_ros - roscpp -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -find_package(gazebo REQUIRED) +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(rclcpp REQUIRED) +find_package(control_toolbox REQUIRED) +find_package(control_msgs REQUIRED) +find_package(gazebo_ros REQUIRED) -catkin_package( - INCLUDE_DIRS - include - LIBRARIES - roboticsgroup_upatras_gazebo_mimic_joint_plugin - roboticsgroup_upatras_gazebo_disable_link_plugin - CATKIN_DEPENDS - control_toolbox - gazebo_ros - roscpp +add_library(roboticsgroup_upatras_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp) +target_compile_features(roboticsgroup_upatras_gazebo_mimic_joint_plugin PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(roboticsgroup_upatras_gazebo_mimic_joint_plugin PUBLIC + $ + $) +ament_target_dependencies( + roboticsgroup_upatras_gazebo_mimic_joint_plugin + "rclcpp" + "control_toolbox" + "control_msgs" + "gazebo_ros" ) -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${GAZEBO_INCLUDE_DIRS} -) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(roboticsgroup_upatras_gazebo_mimic_joint_plugin PRIVATE "ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_BUILDING_LIBRARY") -link_directories(${GAZEBO_LIBRARY_DIRS}) +add_library(roboticsgroup_upatras_gazebo_disable_link_plugin src/disable_link_plugin.cpp) +target_compile_features(roboticsgroup_upatras_gazebo_disable_link_plugin PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(roboticsgroup_upatras_gazebo_disable_link_plugin PUBLIC + $ + $) +ament_target_dependencies( + roboticsgroup_upatras_gazebo_disable_link_plugin + "rclcpp" + "control_toolbox" + "gazebo_ros" +) -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(roboticsgroup_upatras_gazebo_disable_link_plugin PRIVATE "ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_BUILDING_LIBRARY") -add_library(roboticsgroup_upatras_gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp) -target_link_libraries(roboticsgroup_upatras_gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) +install( + TARGETS + roboticsgroup_upatras_gazebo_mimic_joint_plugin + roboticsgroup_upatras_gazebo_disable_link_plugin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) -add_library(roboticsgroup_upatras_gazebo_disable_link_plugin src/disable_link_plugin.cpp) -target_link_libraries(roboticsgroup_upatras_gazebo_disable_link_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() -install(TARGETS +ament_export_include_directories( + include +) +ament_export_libraries( roboticsgroup_upatras_gazebo_mimic_joint_plugin roboticsgroup_upatras_gazebo_disable_link_plugin - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY include/${PROJECT_NAME} - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +ament_package() diff --git a/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h b/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h index da9bf05..2102ff3 100644 --- a/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h +++ b/include/roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h @@ -24,19 +24,26 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN // ROS includes -#include +#include // Gazebo includes #include #include +// Visibility control +#include + namespace gazebo { class DisableLinkPlugin : public ModelPlugin { public: + ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC DisableLinkPlugin(); + + ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC virtual ~DisableLinkPlugin() override; + ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override; private: diff --git a/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h b/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h index 86a8da6..ce90354 100644 --- a/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h +++ b/include/roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h @@ -24,22 +24,29 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN // ROS includes -#include +#include // ros_control -#include +#include // Gazebo includes #include #include +// Visibility control +#include + namespace gazebo { class MimicJointPlugin : public ModelPlugin { public: + ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC MimicJointPlugin(); + + ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC virtual ~MimicJointPlugin() override; + ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) override; private: @@ -51,7 +58,7 @@ namespace gazebo { bool has_pid_; // PID controller if needed - control_toolbox::Pid pid_; + std::unique_ptr pid_; // Pointers to the joints physics::JointPtr joint_, mimic_joint_; diff --git a/include/roboticsgroup_upatras_gazebo_plugins/visibility_control.h b/include/roboticsgroup_upatras_gazebo_plugins/visibility_control.h new file mode 100644 index 0000000..b3c82f2 --- /dev/null +++ b/include/roboticsgroup_upatras_gazebo_plugins/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS__VISIBILITY_CONTROL_H_ +#define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT __attribute__ ((dllexport)) + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT __attribute__ ((dllimport)) + #else + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT __declspec(dllexport) + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT __declspec(dllimport) + #endif + #ifdef ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_BUILDING_LIBRARY + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT + #else + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT + #endif + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC_TYPE ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_LOCAL +#else + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_EXPORT __attribute__ ((visibility("default"))) + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_IMPORT + #if __GNUC__ >= 4 + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC __attribute__ ((visibility("default"))) + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_LOCAL + #endif + #define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_PUBLIC_TYPE +#endif + +#endif // ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS__VISIBILITY_CONTROL_H_ diff --git a/package.xml b/package.xml index bf45f47..fe30969 100644 --- a/package.xml +++ b/package.xml @@ -1,4 +1,5 @@ + -> + roboticsgroup_upatras_gazebo_plugins - 0.2.0 + 0.2.1 Collection of gazebo plugins Konstantinos Chatzilygeroudis @@ -32,18 +33,17 @@ OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISE https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins/issues - catkin - control_toolbox - gazebo_ros - roscpp - control_toolbox - gazebo_ros - roscpp - control_toolbox - gazebo_ros - roscpp + ament_cmake_ros + + rclcpp + control_toolbox + control_msgs + gazebo_ros + + ament_lint_auto + ament_lint_common - + ament_cmake diff --git a/src/disable_link_plugin.cpp b/src/disable_link_plugin.cpp index 5981846..b742f4b 100644 --- a/src/disable_link_plugin.cpp +++ b/src/disable_link_plugin.cpp @@ -20,6 +20,7 @@ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. **/ +#include #include namespace gazebo { @@ -40,7 +41,7 @@ namespace gazebo { // Check for link element if (!_sdf->HasElement("link")) { - ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded."); + RCLCPP_ERROR(rclcpp::get_logger("disable_link_plugin"),"No link element present. DisableLinkPlugin could not be loaded."); return; } @@ -51,12 +52,12 @@ namespace gazebo { if (link_) { link_->SetEnabled(false); // Output some confirmation - ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: " << link_name_); + RCLCPP_INFO_STREAM(rclcpp::get_logger("disable_link_plugin"),"DisableLinkPlugin loaded! Link: " << link_name_); } else - ROS_ERROR_STREAM("Link " << link_name_ << " not found! DisableLinkPlugin could not be loaded."); + RCLCPP_ERROR_STREAM(rclcpp::get_logger("disable_link_plugin"),"Link " << link_name_ << " not found! DisableLinkPlugin could not be loaded."); } - GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin); + GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin) } // namespace gazebo diff --git a/src/mimic_joint_plugin.cpp b/src/mimic_joint_plugin.cpp index 48b588b..70f8c58 100644 --- a/src/mimic_joint_plugin.cpp +++ b/src/mimic_joint_plugin.cpp @@ -20,6 +20,13 @@ INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. **/ +#include + +#include +#include + +#include + #include #if GAZEBO_MAJOR_VERSION >= 8 @@ -48,13 +55,13 @@ namespace gazebo { // Error message if the model couldn't be found if (!model_) { - ROS_ERROR("Parent model is NULL! MimicJointPlugin could not be loaded."); + RCLCPP_ERROR(rclcpp::get_logger("mimic_joint_plugin"),"Parent model is NULL! MimicJointPlugin could not be loaded."); return; } // Check that ROS has been initialized - if (!ros::isInitialized()) { - ROS_ERROR("A ROS node for Gazebo has not been initialized, unable to load plugin."); + if (!rclcpp::ok()) { + RCLCPP_ERROR(rclcpp::get_logger("mimic_joint_plugin"),"A ROS node for Gazebo has not been initialized, unable to load plugin."); return; } @@ -62,11 +69,11 @@ namespace gazebo { if (_sdf->HasElement("robotNamespace")) { robot_namespace_ = _sdf->GetElement("robotNamespace")->Get(); } - ros::NodeHandle model_nh(robot_namespace_); + rclcpp::Node::SharedPtr model_nh=gazebo_ros::Node::Get(_sdf); // Check for joint element if (!_sdf->HasElement("joint")) { - ROS_ERROR("No joint element present. MimicJointPlugin could not be loaded."); + RCLCPP_ERROR(model_nh->get_logger(),"No joint element present. MimicJointPlugin could not be loaded."); return; } @@ -74,7 +81,7 @@ namespace gazebo { // Check for mimicJoint element if (!_sdf->HasElement("mimicJoint")) { - ROS_ERROR("No mimicJoint element present. MimicJointPlugin could not be loaded."); + RCLCPP_ERROR(model_nh->get_logger(),"No mimicJoint element present. MimicJointPlugin could not be loaded."); return; } @@ -87,8 +94,9 @@ namespace gazebo { if (name.empty()) { name = "gazebo_ros_control/pid_gains/" + mimic_joint_name_; } - const ros::NodeHandle nh(model_nh, name); - pid_.init(nh); + rclcpp::Node::SharedPtr nh=std::make_shared(name,model_nh->get_namespace()); + pid_=std::make_unique(nh); + pid_->initPid(); } // Check for multiplier element @@ -109,12 +117,12 @@ namespace gazebo { // Get pointers to joints joint_ = model_->GetJoint(joint_name_); if (!joint_) { - ROS_ERROR_STREAM("No joint named \"" << joint_name_ << "\". MimicJointPlugin could not be loaded."); + RCLCPP_ERROR_STREAM(model_nh->get_logger(),"No joint named \"" << joint_name_ << "\". MimicJointPlugin could not be loaded."); return; } mimic_joint_ = model_->GetJoint(mimic_joint_name_); if (!mimic_joint_) { - ROS_ERROR_STREAM("No (mimic) joint named \"" << mimic_joint_name_ << "\". MimicJointPlugin could not be loaded."); + RCLCPP_ERROR_STREAM(model_nh->get_logger(),"No (mimic) joint named \"" << mimic_joint_name_ << "\". MimicJointPlugin could not be loaded."); return; } @@ -143,17 +151,18 @@ namespace gazebo { boost::bind(&MimicJointPlugin::UpdateChild, this)); // Output some confirmation - ROS_INFO_STREAM("MimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\"" + RCLCPP_INFO_STREAM(model_nh->get_logger(),"MimicJointPlugin loaded! Joint: \"" << joint_name_ << "\", Mimic joint: \"" << mimic_joint_name_ << "\"" << ", Multiplier: " << multiplier_ << ", Offset: " << offset_ << ", MaxEffort: " << max_effort_ << ", Sensitiveness: " << sensitiveness_); } void MimicJointPlugin::UpdateChild() { + using namespace std::chrono_literals; #if GAZEBO_MAJOR_VERSION >= 8 - static ros::Duration period(world_->Physics()->GetMaxStepSize()); + static rclcpp::Duration period(world_->Physics()->GetMaxStepSize()*1s); #else - static ros::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize()); + static rclcpp::Duration period(world_->GetPhysicsEngine()->GetMaxStepSize()*1s); #endif // Set mimic joint's angle based on joint's angle @@ -170,17 +179,17 @@ namespace gazebo { if (a != a) a = angle; double error = angle - a; - double effort = math::clamp(pid_.computeCommand(error, period), -max_effort_, max_effort_); + double effort = math::clamp(pid_->computeCommand(error, period), -max_effort_, max_effort_); mimic_joint_->SetForce(0, effort); } else { #if GAZEBO_MAJOR_VERSION >= 9 mimic_joint_->SetPosition(0, angle, true); #elif GAZEBO_MAJOR_VERSION > 2 - ROS_WARN_ONCE("The mimic_joint plugin is using the Joint::SetPosition method without preserving the link velocity."); - ROS_WARN_ONCE("As a result, gravity will not be simulated correctly for your model."); - ROS_WARN_ONCE("Please set gazebo_pid parameters or upgrade to Gazebo 9."); - ROS_WARN_ONCE("For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); + RCLCPP_WARN_ONCE(model_nh->get_logger(),"The mimic_joint plugin is using the Joint::SetPosition method without preserving the link velocity."); + RCLCPP_WARN_ONCE(model_nh->get_logger(),"As a result, gravity will not be simulated correctly for your model."); + RCLCPP_WARN_ONCE(model_nh->get_logger(),"Please set gazebo_pid parameters or upgrade to Gazebo 9."); + RCLCPP_WARN_ONCE(model_nh->get_logger(),"For details, see https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612"); mimic_joint_->SetPosition(0, angle); #else mimic_joint_->SetAngle(0, math::Angle(angle)); @@ -189,6 +198,6 @@ namespace gazebo { } } - GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin); + GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin) } // namespace gazebo