-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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>)
+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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<INSTALL_INTERFACE:include>)
+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()
#define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
// ROS includes
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
// Gazebo includes
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
+// Visibility control
+#include <roboticsgroup_upatras_gazebo_plugins/visibility_control.h>
+
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:
#define ROBOTICSGROUP_UPATRAS_GAZEBO_PLUGINS_MIMIC_JOINT_PLUGIN
// ROS includes
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
// ros_control
-#include <control_toolbox/pid.h>
+#include <control_toolbox/pid_ros.hpp>
// Gazebo includes
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
+// Visibility control
+#include <roboticsgroup_upatras_gazebo_plugins/visibility_control.h>
+
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:
bool has_pid_;
// PID controller if needed
- control_toolbox::Pid pid_;
+ std::unique_ptr<control_toolbox::PidROS> pid_;
// Pointers to the joints
physics::JointPtr joint_, mimic_joint_;
--- /dev/null
+#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_
<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -->
-<package format="2">>
+<package format="3">
<name>roboticsgroup_upatras_gazebo_plugins</name>
- <version>0.2.0</version>
+ <version>0.2.1</version>
<description>Collection of gazebo plugins</description>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<url type="repository">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins</url>
<url type="bugtracker">https://github.com/roboticsgroup/roboticsgroup_upatras_gazebo_plugins/issues</url>
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>control_toolbox</build_depend>
- <build_depend>gazebo_ros</build_depend>
- <build_depend>roscpp</build_depend>
- <build_export_depend>control_toolbox</build_export_depend>
- <build_export_depend>gazebo_ros</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <exec_depend>control_toolbox</exec_depend>
- <exec_depend>gazebo_ros</exec_depend>
- <exec_depend>roscpp</exec_depend>
+ <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+ <depend>rclcpp</depend>
+ <depend>control_toolbox</depend>
+ <depend>control_msgs</depend>
+ <depend>gazebo_ros</depend>
+
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
<export>
- <gazebo_ros plugin_path="${prefix}/lib" />
+ <build_type>ament_cmake</build_type>
</export>
</package>
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**/
+#include <rclcpp/logging.hpp>
#include <roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h>
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;
}
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
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
**/
+#include <chrono>
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/logging.hpp>
+
+#include <gazebo_ros/node.hpp>
+
#include <roboticsgroup_upatras_gazebo_plugins/mimic_joint_plugin.h>
#if GAZEBO_MAJOR_VERSION >= 8
// 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;
}
if (_sdf->HasElement("robotNamespace")) {
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
}
- 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;
}
// 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;
}
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<rclcpp::Node>(name,model_nh->get_namespace());
+ pid_=std::make_unique<control_toolbox::PidROS>(nh);
+ pid_->initPid();
}
// Check for multiplier element
// 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;
}
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
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));
}
}
- GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin);
+ GZ_REGISTER_MODEL_PLUGIN(MimicJointPlugin)
} // namespace gazebo
projects / roboticsgroup_upatras_gazebo_plugins.git / commitdiff