pal-robotics · davidorchansky · Dec 5, 2018 · Dec 5, 2018 · Nov 26, 2021 · May 22, 2023
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -1,86 +1,112 @@
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(gazebo_ros_link_attacher)
 
-#####################################
-## Check c++11 / c++0x support ######
-#####################################
-include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX11)
-  set(CMAKE_CXX_FLAGS "-std=c++11")
-elseif(COMPILER_SUPPORTS_CXX0X)
-  set(CMAKE_CXX_FLAGS "-std=c++0x")
-else()
-  message(FATAL_ERROR "Compiler ${CMAKE_CXX_COMPILER} has no C++11 support.")
+##########################
+## Set compiler options ##
+##########################
+add_compile_options(-std=c++17)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
-#####################################
-
-# Load catkin and all dependencies required for this package
-find_package(catkin REQUIRED COMPONENTS 
-  roscpp 
-  gazebo_ros
-  std_msgs
-  message_generation
-)
 
-# Depend on system install of Gazebo
-find_package(gazebo REQUIRED)
+set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}") 
 
-add_service_files(
-  FILES
-  Attach.srv
-)
+########################
+
+# Load dependencies required for this package
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED) 
+find_package(gazebo_ros REQUIRED) 
+find_package(std_msgs REQUIRED) 
+find_package(gazebo REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(rmw_implementation_cmake REQUIRED)
 
 
 ## Generate added messages and services with any dependencies listed here
-generate_messages(
-  DEPENDENCIES
-  std_msgs
+rosidl_generate_interfaces(${PROJECT_NAME}
+  srv/Attach.srv
+  DEPENDENCIES builtin_interfaces std_msgs
 )
 
-###################################
-## catkin specific configuration ##
-###################################
-catkin_package(CATKIN_DEPENDS message_runtime)
+ament_export_dependencies(rosidl_default_runtime)
+
 
 ###########
 ## Build ##
 ###########
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-include_directories(include)
-link_directories(${GAZEBO_LIBRARY_DIRS})
-include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE_DIRS})
+include_directories(include
+  ${gazebo_ros_INCLUDE_DIRS}
+  ${rclcpp_INCLUDE_DIRS}
+  ${std_msgs_INCLUDE_DIRS}
+  ${CMAKE_CURRENT_BINARY_DIR}/srv
+)
 
 ## Declare a cpp library
-add_library(${PROJECT_NAME} src/gazebo_ros_link_attacher.cpp)
+add_library(${PROJECT_NAME}_plugin SHARED src/gazebo_ros_link_attacher.cpp)
+
 
+# Connect targets with locally defined messages
+get_available_rmw_implementations(rmw_implementations2)
+foreach(rmw_implementation ${rmw_implementations2})
+  find_package("${rmw_implementation}" REQUIRED)
+  get_rmw_typesupport(typesupport_impls "${rmw_implementation}" LANGUAGE "cpp")
+  foreach(typesupport_impl ${typesupport_impls})
+    rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp")
+  endforeach()
+endforeach()
 
 ## Specify libraries to link a library or executable target against
-target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
-add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS})
+target_link_libraries(${PROJECT_NAME}_plugin 
+  ${ament_cmake_LIBRARIES} 
+  ${gazebo_ros_LIBRARIES} 
+  ${rclcpp_LIBRARIES}
+  ${std_msgs_LIBRARIES}
+  "${cpp_typesupport_target}")
 
 
 #############
 ## Install ##
 #############
 
+install(TARGETS ${PROJECT_NAME}_plugin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
 install(PROGRAMS
   scripts/demo.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION lib/${PROJECT_NAME}
 )
 
-## Mark executables and/or libraries for installation
-install(TARGETS ${PROJECT_NAME}
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+install(PROGRAMS
+  scripts/demo_multiple.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  scripts/spawn_models.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+install(PROGRAMS
+  scripts/attach.py
+  DESTINATION lib/${PROJECT_NAME}
 )
 
+install(PROGRAMS
+  scripts/detach.py
+  DESTINATION lib/${PROJECT_NAME}
+)
 
-foreach (dir launch)
+foreach (dir launch worlds)
   install(DIRECTORY ${dir}/
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+  DESTINATION share/${PROJECT_NAME}/${dir})
 endforeach(dir)
+
+ament_package()
diff --git a/README.md b/README.md
@@ -8,14 +8,20 @@ cd gazebo_link_attacher_ws/src
 catkin_init_workspace
 git clone https://github.com/pal-robotics/gazebo_ros_link_attacher.git
 cd ..
-catkin_make
-source devel/setup.bash
+
+rosdep update
+rosdep install --from-paths src --ignore-src -y --rosdistro humble
+
+colcon build
+source install/setup.bash
+
+
 ````
 
 
 # Launch
 
-    roslaunch gazebo_ros_link_attacher test_attacher.launch
+    ros2 launch gazebo_ros_link_attacher test_attacher.py
 
 Empty world with the plugin `libgazebo_ros_link_attacher.so` loaded (in the *worlds* folder).
 
@@ -27,34 +33,34 @@ And `/link_attacher_node/detach` service to specify two models and their links t
 
 # Run demo
 
-In another shell, be sure to do `source devel/setup.bash` of your workspace.
+In another shell, be sure to do `source install/setup.bash` of your workspace.
 
-    rosrun gazebo_ros_link_attacher spawn.py
+    ros2 run gazebo_ros_link_attacher spawn_models.py
 
 Three cubes will be spawned.
 
-    rosrun gazebo_ros_link_attacher attach.py
+    ros2 run gazebo_ros_link_attacher attach.py
 
 The cubes will be attached all between themselves as (1,2), (2,3), (3,1). You can move them with the GUI and you'll see they will move together.
 
-    rosrun gazebo_ros_link_attacher detach.py
+    ros2 run gazebo_ros_link_attacher detach.py
 
 The cubes will be detached and you can move them separately again.
 
 You can also spawn items with the GUI and run a rosservice call:
 ````
-rosservice call /link_attacher_node/attach "model_name_1: 'unit_box_1'
-link_name_1: 'link'
-model_name_2: 'unit_sphere_1'
-link_name_2: 'link'"
+ros2 service call /attach 'gazebo_ros_link_attacher/srv/Attach' '{model_name_1: 'cube1',
+link_name_1: 'link',
+model_name_2: 'cube2',
+link_name_2: 'link'}'
 ````
 
 And same thing to detach:
 ````
-rosservice call /link_attacher_node/detach "model_name_1: 'unit_box_1'
-link_name_1: 'link'
-model_name_2: 'unit_sphere_1'
-link_name_2: 'link'"
+ros2 service call /detach 'gazebo_ros_link_attacher/srv/Attach' '{model_name_1: 'cube1',
+link_name_1: 'link',
+model_name_2: 'cube2',
+link_name_2: 'link'}'
 ````
 
 
@@ -77,4 +83,4 @@ Aborted (core dumped)
 [Gripper.hh](https://bitbucket.org/osrf/gazebo/src/1d1e3a542af81670f43a120e1df7190592bc4c0f/gazebo/physics/Gripper.hh?at=default&fileviewer=file-view-default)
 [Gripper.cc](https://bitbucket.org/osrf/gazebo/src/1d1e3a542af81670f43a120e1df7190592bc4c0f/gazebo/physics/Gripper.cc?at=default&fileviewer=file-view-default)~~
 
-~~Which is to create a revolute joint on the first model (with range of motion 0) linking a link on the first model and a link on the second model.~~
+~~Which is to create a revolute joint on the first model (with range of motion 0) linking a link on the first model and a link on the second model.~~
diff --git a/include/gazebo_ros_link_attacher.h b/include/gazebo_ros_link_attacher.h
@@ -7,10 +7,11 @@
 #ifndef GAZEBO_ROS_LINK_ATTACHER_HH
 #define GAZEBO_ROS_LINK_ATTACHER_HH
 
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
 
 #include <sdf/sdf.hh>
 #include "gazebo/gazebo.hh"
+#include <gazebo_ros/node.hpp>
 #include <gazebo/physics/physics.hh>
 #include "gazebo/physics/PhysicsTypes.hh"
 #include <gazebo/transport/TransportTypes.hh>
@@ -19,9 +20,7 @@
 #include <gazebo/common/Events.hh>
 #include "gazebo/msgs/msgs.hh"
 #include "gazebo/transport/transport.hh"
-#include "gazebo_ros_link_attacher/Attach.h"
-#include "gazebo_ros_link_attacher/AttachRequest.h"
-#include "gazebo_ros_link_attacher/AttachResponse.h"
+#include "gazebo_ros_link_attacher/srv/attach.hpp"
 
 namespace gazebo
 {
@@ -36,7 +35,7 @@ namespace gazebo
         virtual ~GazeboRosLinkAttacher();
 
         /// \brief Load the controller
-        void Load( physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/ );
+        void Load( physics::WorldPtr _world, sdf::ElementPtr _sdf);
 
         /// \brief Attach with a revolute joint
         bool attach(std::string model1, std::string link1,
@@ -62,14 +61,14 @@ namespace gazebo
         bool getJoint(std::string model1, std::string link1, std::string model2, std::string link2, fixedJoint &joint);
 
    private:
-        ros::NodeHandle nh_;
-        ros::ServiceServer attach_service_;
-        ros::ServiceServer detach_service_;
-
-        bool attach_callback(gazebo_ros_link_attacher::Attach::Request &req,
-                              gazebo_ros_link_attacher::Attach::Response &res);
-        bool detach_callback(gazebo_ros_link_attacher::Attach::Request &req,
-                             gazebo_ros_link_attacher::Attach::Response &res);
+        gazebo_ros::Node::SharedPtr node;
+        rclcpp::Service<gazebo_ros_link_attacher::srv::Attach>::SharedPtr attach_service_;
+        rclcpp::Service<gazebo_ros_link_attacher::srv::Attach>::SharedPtr detach_service_;
+
+        bool attach_callback(const std::shared_ptr<gazebo_ros_link_attacher::srv::Attach::Request> req,
+                              std::shared_ptr<gazebo_ros_link_attacher::srv::Attach::Response> res);
+        bool detach_callback(const std::shared_ptr<gazebo_ros_link_attacher::srv::Attach::Request> req,
+                             std::shared_ptr<gazebo_ros_link_attacher::srv::Attach::Response> res);
 
         std::vector<fixedJoint> joints;
 

diff --git a/launch/test_attacher.launch b/launch/test_attacher.launch
diff --git a/launch/test_attacher.py b/launch/test_attacher.py
@@ -0,0 +1,41 @@
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.actions import IncludeLaunchDescription
+from launch.actions import GroupAction
+from launch.actions import OpaqueFunction
+
+import os
+import pathlib
+
+def launch_setup(context, *args, **kwargs): 
+    # Start Gazebo with an empty world file
+    gazebo_launch = os.path.join(get_package_share_directory('gazebo_ros'), 'launch/gazebo.launch.py')
+    world_file = os.path.join(get_package_share_directory('gazebo_ros_link_attacher'), 'worlds/test_attacher.world')
+
+    if not pathlib.Path(world_file).exists():
+        exc = 'World file ' + world_file + ' does not exist'
+        raise Exception(exc)
+
+    launch_args = [('world', world_file), 
+                   ('verbose', 'true'),
+                   ('debug', 'true'),
+                   ('gui', 'true'),
+                   ('pause', 'false')]
+
+    print ("world file: ", world_file)
+    gazebo_launch_description = IncludeLaunchDescription(
+            AnyLaunchDescriptionSource(gazebo_launch), launch_arguments=launch_args)
+
+    group = GroupAction([
+        gazebo_launch_description,
+    ])
+
+    return [group]
+
+
+def generate_launch_description():
+
+    return LaunchDescription([
+        OpaqueFunction(function = launch_setup)
+    ])
diff --git a/package.xml b/package.xml
@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<package>
+<package format="3">
   <name>gazebo_ros_link_attacher</name>
   <version>0.0.0</version>
   <description>The gazebo_ros_link_attacher package lets you attach
@@ -10,19 +10,23 @@
 
   <license>BSD</license>
 
-
-  <build_depend>message_generation</build_depend>
-  <build_depend>roscpp</build_depend>
+  <build_depend>rclcpp</build_depend>
   <build_depend>gazebo_ros</build_depend>
   <build_depend>gazebo</build_depend>
   <build_depend>std_msgs</build_depend>
 
-  <run_depend>message_runtime</run_depend>
-  <run_depend>roscpp</run_depend>
-  <run_depend>gazebo_ros</run_depend>
-  <run_depend>std_msgs</run_depend>
+  <build_depend>python-transforms3d-pip</build_depend>
 
-  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>gazebo_ros</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
 
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
 </package>