diff --git a/CMakeLists.txt b/CMakeLists.txt index f7e63e0..841bfec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,48 +1,37 @@ -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 ## @@ -50,37 +39,68 @@ catkin_package(CATKIN_DEPENDS message_runtime) ## 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_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) ## 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_LIBRARIES}) + +# 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_target_interfaces(${PROJECT_NAME}_plugin ${PROJECT_NAME} ${typesupport_impl}) + endforeach() +endforeach() ############# ## 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 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 index 3aaf5df..4e92f57 100644 --- 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 foxy + +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.~~ \ No newline at end of file +~~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 index 939c869..e3beab3 100644 --- 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 +#include #include #include "gazebo/gazebo.hh" +#include #include #include "gazebo/physics/PhysicsTypes.hh" #include @@ -19,9 +20,7 @@ #include #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::SharedPtr attach_service_; + rclcpp::Service::SharedPtr detach_service_; + + bool attach_callback(const std::shared_ptr req, + std::shared_ptr res); + bool detach_callback(const std::shared_ptr req, + std::shared_ptr res); std::vector joints; diff --git a/launch/test_attacher.launch b/launch/test_attacher.launch deleted file mode 100644 index 361bafc..0000000 --- a/launch/test_attacher.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/launch/test_attacher.py b/launch/test_attacher.py new file mode 100644 index 0000000..04529f9 --- /dev/null +++ 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) + ]) \ No newline at end of file diff --git a/package.xml b/package.xml index 768d02f..6b3d601 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,5 @@ - + gazebo_ros_link_attacher 0.0.0 The gazebo_ros_link_attacher package lets you attach @@ -10,19 +10,23 @@ BSD - - message_generation - roscpp + rclcpp gazebo_ros gazebo std_msgs - message_runtime - roscpp - gazebo_ros - std_msgs + python-transforms3d-pip - catkin + rclcpp + gazebo_ros + std_msgs + rosidl_default_runtime + ament_cmake + + rosidl_interface_packages + + ament_cmake + diff --git a/scripts/attach.py b/scripts/attach.py index 5b7849c..e9a154f 100755 --- a/scripts/attach.py +++ b/scripts/attach.py @@ -1,48 +1,58 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy -from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse +import sys +import rclpy +from gazebo_ros_link_attacher.srv import Attach if __name__ == '__main__': - rospy.init_node('demo_attach_links') - rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach") - attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', - Attach) - attach_srv.wait_for_service() - rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach") + rclpy.init(args=sys.argv) + node = rclpy.create_node('demo_attach_links') + node.get_logger().info("Creating service to /attach") + + attach_srv = node.create_client(Attach, '/attach') + while not attach_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info("Waiting for attach service...") + + node.get_logger().info("Created service to /attach") # Link them - rospy.loginfo("Attaching cube1 and cube2") - req = AttachRequest() + node.get_logger().info("Attaching cube1 and cube2") + + req = Attach.Request() req.model_name_1 = "cube1" req.link_name_1 = "link" req.model_name_2 = "cube2" req.link_name_2 = "link" - attach_srv.call(req) + resp = attach_srv.call_async(req) + rclpy.spin_until_future_complete(node, resp) + # From the shell: """ -rosservice call /link_attacher_node/attach "model_name_1: 'cube1' -link_name_1: 'link' -model_name_2: 'cube2' -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'}' """ - rospy.loginfo("Attaching cube2 and cube3") - req = AttachRequest() + node.get_logger().info("Attaching cube2 and cube3") + + req = Attach.Request() req.model_name_1 = "cube2" req.link_name_1 = "link" req.model_name_2 = "cube3" req.link_name_2 = "link" - attach_srv.call(req) + resp = attach_srv.call_async(req) + rclpy.spin_until_future_complete(node, resp) - rospy.loginfo("Attaching cube3 and cube1") - req = AttachRequest() + node.get_logger().info("Attaching cube3 and cube1") + req = Attach.Request() req.model_name_1 = "cube3" req.link_name_1 = "link" req.model_name_2 = "cube1" req.link_name_2 = "link" - attach_srv.call(req) + resp = attach_srv.call_async(req) + rclpy.spin_until_future_complete(node, resp) \ No newline at end of file diff --git a/scripts/demo.py b/scripts/demo.py index f04fd4f..edaf3e4 100755 --- a/scripts/demo.py +++ b/scripts/demo.py @@ -1,10 +1,11 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy -from gazebo_ros_link_attacher.msg import Attach -from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse +import sys +import rclpy +from gazebo_ros_link_attacher.srv import Attach +from gazebo_msgs.srv import SpawnEntity from copy import deepcopy -from tf.transformations import quaternion_from_euler +from transforms3d.euler import euler2quat sdf_cube = """ @@ -88,14 +89,14 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): # Replace modelname cube = cube.replace('MODELNAME', str(modelname)) - req = SpawnModelRequest() - req.model_name = modelname - req.model_xml = cube + req = SpawnEntity.Request() + req.name = modelname + req.xml = cube req.initial_pose.position.x = px req.initial_pose.position.y = py req.initial_pose.position.z = pz - q = quaternion_from_euler(rr, rp, ry) + q = euler2quat(rr, rp, ry) req.initial_pose.orientation.x = q[0] req.initial_pose.orientation.y = q[1] req.initial_pose.orientation.z = q[2] @@ -105,47 +106,53 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): if __name__ == '__main__': - rospy.init_node('demo_attach_links') - attach_pub = rospy.Publisher('/link_attacher_node/attach_models', - Attach, queue_size=1) - rospy.loginfo("Created publisher to /link_attacher_node/attach_models") - spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) - rospy.loginfo("Waiting for /gazebo/spawn_sdf_model service...") - spawn_srv.wait_for_service() - rospy.loginfo("Connected to service!") + rclpy.init(args=sys.argv) + node = rclpy.create_node('demo_attach_links') + + attach_srv = node.create_client(Attach, '/attach') + while not attach_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info(f'Waiting for service {attach_srv.srv_name}...') + + service_name = 'spawn_entity' + spawn_srv = node.create_client(SpawnEntity, '/spawn_entity') + while not spawn_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info(f'Waiting for service {spawn_srv.srv_name}...') + + node.get_logger().info("Connected to service!") # Spawn object 1 - rospy.loginfo("Spawning cube1") + node.get_logger().info("Spawning cube1") req1 = create_cube_request("cube1", 0.0, 0.0, 0.51, # position 0.0, 0.0, 0.0, # rotation 1.0, 1.0, 1.0) # size - spawn_srv.call(req1) - rospy.sleep(1.0) + resp = spawn_srv.call_async(req1) + rclpy.spin_until_future_complete(node, resp) # Spawn object 2 - rospy.loginfo("Spawning cube2") + node.get_logger().info("Spawning cube2") req2 = create_cube_request("cube2", 0.0, 1.1, 0.41, # position 0.0, 0.0, 0.0, # rotation 0.8, 0.8, 0.8) # size - spawn_srv.call(req2) - rospy.sleep(1.0) + resp = spawn_srv.call_async(req2) + rclpy.spin_until_future_complete(node, resp) # Link them - rospy.loginfo("Attaching cube1 and cube2") - amsg = Attach() + node.get_logger().info("Attaching cube1 and cube2") + amsg = Attach.Request() amsg.model_name_1 = "cube1" amsg.link_name_1 = "link" amsg.model_name_2 = "cube2" amsg.link_name_2 = "link" - attach_pub.publish(amsg) + resp = attach_srv.call_async(amsg) + rclpy.spin_until_future_complete(node, resp) # From the shell: """ -rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1' -link_name_1: 'link' -model_name_2: 'cube2' -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'}' """ - rospy.loginfo("Published into linking service!") + node.get_logger().info("Published into linking service!") diff --git a/scripts/demo_multiple.py b/scripts/demo_multiple.py index 93575fc..99fb9f8 100755 --- a/scripts/demo_multiple.py +++ b/scripts/demo_multiple.py @@ -1,10 +1,11 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy -from gazebo_ros_link_attacher.msg import Attach -from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse +import sys +import rclpy +from gazebo_msgs.srv import SpawnEntity +from gazebo_ros_link_attacher.srv import Attach from copy import deepcopy -from tf.transformations import quaternion_from_euler +from transforms3d.euler import euler2quat sdf_cube = """ @@ -88,14 +89,14 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): # Replace modelname cube = cube.replace('MODELNAME', str(modelname)) - req = SpawnModelRequest() - req.model_name = modelname - req.model_xml = cube + req = SpawnEntity.Request() + req.name = modelname + req.xml = cube req.initial_pose.position.x = px req.initial_pose.position.y = py req.initial_pose.position.z = pz - q = quaternion_from_euler(rr, rp, ry) + q = euler2quat(rr, rp, ry) req.initial_pose.orientation.x = q[0] req.initial_pose.orientation.y = q[1] req.initial_pose.orientation.z = q[2] @@ -105,79 +106,86 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): if __name__ == '__main__': - rospy.init_node('demo_attach_links') - attach_pub = rospy.Publisher('/link_attacher_node/attach_models', - Attach, queue_size=1) - rospy.loginfo("Created publisher to /link_attacher_node/attach_models") - spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) - rospy.loginfo("Waiting for /gazebo/spawn_sdf_model service...") - spawn_srv.wait_for_service() - rospy.loginfo("Connected to service!") + rclpy.init(args=sys.argv) + node = rclpy.create_node('demo_attach_links') + + attach_srv = node.create_client(Attach, '/attach') + while not attach_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info(f'Waiting for service {attach_srv.srv_name}...') + + service_name = 'spawn_entity' + spawn_srv = node.create_client(SpawnEntity, '/spawn_entity') + while not spawn_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info(f'Waiting for service {spawn_srv.srv_name}...') + + node.get_logger().info("Connected to service!") # Spawn object 1 - rospy.loginfo("Spawning cube1") + node.get_logger().info("Spawning cube1") req1 = create_cube_request("cube1", 0.0, 0.0, 0.51, # position 0.0, 0.0, 0.0, # rotation 1.0, 1.0, 1.0) # size - spawn_srv.call(req1) - rospy.sleep(1.0) + resp = spawn_srv.call_async(req1) + rclpy.spin_until_future_complete(node, resp) # Spawn object 2 - rospy.loginfo("Spawning cube2") + node.get_logger().info("Spawning cube2") req2 = create_cube_request("cube2", 0.0, 1.1, 0.41, # position 0.0, 0.0, 0.0, # rotation 0.8, 0.8, 0.8) # size - spawn_srv.call(req2) - rospy.sleep(1.0) + resp = spawn_srv.call_async(req2) + rclpy.spin_until_future_complete(node, resp) # Spawn object 3 - rospy.loginfo("Spawning cube3") + node.get_logger().info("Spawning cube3") req3 = create_cube_request("cube3", 0.0, 2.1, 0.41, # position 0.0, 0.0, 0.0, # rotation 0.4, 0.4, 0.4) # size - spawn_srv.call(req3) - rospy.sleep(1.0) + + resp = spawn_srv.call_async(req3) + rclpy.spin_until_future_complete(node, resp) # Link them - rospy.loginfo("Attaching cube1 and cube2") - amsg = Attach() + node.get_logger().info("Attaching cube1 and cube2") + + amsg = Attach.Request() amsg.model_name_1 = "cube1" amsg.link_name_1 = "link" amsg.model_name_2 = "cube2" amsg.link_name_2 = "link" - attach_pub.publish(amsg) - rospy.sleep(1.0) + resp = attach_srv.call_async(amsg) + rclpy.spin_until_future_complete(node, resp) # From the shell: """ -rostopic pub /link_attacher_node/attach_models gazebo_ros_link_attacher/Attach "model_name_1: 'cube1' -link_name_1: 'link' -model_name_2: 'cube2' -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'}' """ - rospy.loginfo("Published into linking service!") + node.get_logger().info("Published into linking service!") - rospy.loginfo("Attaching cube2 and cube3") - amsg = Attach() + node.get_logger().info("Attaching cube2 and cube3") + amsg = Attach.Request() amsg.model_name_1 = "cube2" amsg.link_name_1 = "link" amsg.model_name_2 = "cube3" amsg.link_name_2 = "link" - attach_pub.publish(amsg) - rospy.sleep(1.0) + resp = attach_srv.call_async(amsg) + rclpy.spin_until_future_complete(node, resp) - rospy.loginfo("Attaching cube3 and cube1") - amsg = Attach() + node.get_logger().info("Attaching cube3 and cube1") + amsg = Attach.Request() amsg.model_name_1 = "cube3" amsg.link_name_1 = "link" amsg.model_name_2 = "cube1" amsg.link_name_2 = "link" - attach_pub.publish(amsg) - rospy.sleep(2.0) + resp = attach_srv.call_async(amsg) + rclpy.spin_until_future_complete(node, resp) diff --git a/scripts/detach.py b/scripts/detach.py index f4d5641..be62464 100755 --- a/scripts/detach.py +++ b/scripts/detach.py @@ -1,48 +1,55 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy -from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse +import sys +import rclpy +from gazebo_ros_link_attacher.srv import Attach if __name__ == '__main__': - rospy.init_node('demo_detach_links') - rospy.loginfo("Creating ServiceProxy to /link_attacher_node/detach") - attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', - Attach) - attach_srv.wait_for_service() - rospy.loginfo("Created ServiceProxy to /link_attacher_node/detach") - - # Link them - rospy.loginfo("Detaching cube1 and cube2") - req = AttachRequest() + rclpy.init(args=sys.argv) + node = rclpy.create_node('demo_detach_links') + node.get_logger().info("Creating service to /detach") + + + attach_srv = node.create_client(Attach, '/detach') + while not attach_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info("Waiting for detach service...") + + # Unlink them + node.get_logger().info("Detaching cube1 and cube2") + req = Attach.Request() req.model_name_1 = "cube1" req.link_name_1 = "link" req.model_name_2 = "cube2" req.link_name_2 = "link" - attach_srv.call(req) + resp = attach_srv.call_async(req) + rclpy.spin_until_future_complete(node, resp) + # From the shell: """ -rosservice call /link_attacher_node/detach "model_name_1: 'cube1' -link_name_1: 'link' -model_name_2: 'cube2' -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'}' """ - rospy.loginfo("Attaching cube2 and cube3") - req = AttachRequest() + node.get_logger().info("Detaching cube2 and cube3") + req = Attach.Request() req.model_name_1 = "cube2" req.link_name_1 = "link" req.model_name_2 = "cube3" req.link_name_2 = "link" - attach_srv.call(req) + resp = attach_srv.call_async(req) + rclpy.spin_until_future_complete(node, resp) - rospy.loginfo("Attaching cube3 and cube1") - req = AttachRequest() + node.get_logger().info("Detaching cube3 and cube1") + req = Attach.Request() req.model_name_1 = "cube3" req.link_name_1 = "link" req.model_name_2 = "cube1" req.link_name_2 = "link" - attach_srv.call(req) + resp = attach_srv.call_async(req) + rclpy.spin_until_future_complete(node, resp) \ No newline at end of file diff --git a/scripts/spawn_models.py b/scripts/spawn_models.py index 52c8df4..6412171 100755 --- a/scripts/spawn_models.py +++ b/scripts/spawn_models.py @@ -1,9 +1,10 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import rospy -from gazebo_msgs.srv import SpawnModel, SpawnModelRequest, SpawnModelResponse +import sys +import rclpy +from gazebo_msgs.srv import SpawnEntity from copy import deepcopy -from tf.transformations import quaternion_from_euler +from transforms3d.euler import euler2quat sdf_cube = """ @@ -74,7 +75,7 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): - """Create a SpawnModelRequest with the parameters of the cube given. + """Create a SpawnEntityRequest with the parameters of the cube given. modelname: name of the model for gazebo px py pz: position of the cube (and it's collision cube) rr rp ry: rotation (roll, pitch, yaw) of the model @@ -87,14 +88,14 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): # Replace modelname cube = cube.replace('MODELNAME', str(modelname)) - req = SpawnModelRequest() - req.model_name = modelname - req.model_xml = cube + req = SpawnEntity.Request() + req.name = modelname + req.xml = cube req.initial_pose.position.x = px req.initial_pose.position.y = py req.initial_pose.position.z = pz - q = quaternion_from_euler(rr, rp, ry) + q = euler2quat(rr, rp, ry) req.initial_pose.orientation.x = q[0] req.initial_pose.orientation.y = q[1] req.initial_pose.orientation.z = q[2] @@ -104,35 +105,47 @@ def create_cube_request(modelname, px, py, pz, rr, rp, ry, sx, sy, sz): if __name__ == '__main__': - rospy.init_node('spawn_models') - spawn_srv = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) - rospy.loginfo("Waiting for /gazebo/spawn_sdf_model service...") - spawn_srv.wait_for_service() - rospy.loginfo("Connected to service!") + rclpy.init(args=sys.argv) + node = rclpy.create_node('spawn_entities') + + service_name = 'spawn_entity' + spawn_srv = node.create_client(SpawnEntity, '/spawn_entity') + + # while not spawn_srv.service_is_ready(): + while not spawn_srv.wait_for_service(timeout_sec=1.0): + node.get_logger().info(f'Waiting for service {spawn_srv.srv_name}...') + + node.get_logger().info("Connected to service!") # Spawn object 1 - rospy.loginfo("Spawning cube1") + node.get_logger().info("Spawning cube1") req1 = create_cube_request("cube1", 0.0, 0.0, 0.51, # position 0.0, 0.0, 0.0, # rotation 1.0, 1.0, 1.0) # size - spawn_srv.call(req1) - rospy.sleep(1.0) + + resp = spawn_srv.call_async(req1) + rclpy.spin_until_future_complete(node, resp) # Spawn object 2 - rospy.loginfo("Spawning cube2") + node.get_logger().info("Spawning cube2") req2 = create_cube_request("cube2", 0.0, 1.1, 0.41, # position 0.0, 0.0, 0.0, # rotation 0.8, 0.8, 0.8) # size - spawn_srv.call(req2) - rospy.sleep(1.0) + + resp = spawn_srv.call_async(req2) + rclpy.spin_until_future_complete(node, resp) # Spawn object 3 - rospy.loginfo("Spawning cube3") + node.get_logger().info("Spawning cube3") req3 = create_cube_request("cube3", 0.0, 2.1, 0.41, # position 0.0, 0.0, 0.0, # rotation 0.4, 0.4, 0.4) # size - spawn_srv.call(req3) - rospy.sleep(1.0) + + resp = spawn_srv.call_async(req3) + rclpy.spin_until_future_complete(node, resp) + + node.destroy_node() + rclpy.shutdown() diff --git a/src/gazebo_ros_link_attacher.cpp b/src/gazebo_ros_link_attacher.cpp index f9b7dbf..d07b65b 100644 --- a/src/gazebo_ros_link_attacher.cpp +++ b/src/gazebo_ros_link_attacher.cpp @@ -1,9 +1,5 @@ #include -#include #include "gazebo_ros_link_attacher.h" -#include "gazebo_ros_link_attacher/Attach.h" -#include "gazebo_ros_link_attacher/AttachRequest.h" -#include "gazebo_ros_link_attacher/AttachResponse.h" #include namespace gazebo @@ -12,8 +8,7 @@ namespace gazebo GZ_REGISTER_WORLD_PLUGIN(GazeboRosLinkAttacher) // Constructor - GazeboRosLinkAttacher::GazeboRosLinkAttacher() : - nh_("link_attacher_node") + GazeboRosLinkAttacher::GazeboRosLinkAttacher() { } @@ -23,23 +18,27 @@ namespace gazebo { } - void GazeboRosLinkAttacher::Load(physics::WorldPtr _world, sdf::ElementPtr /*_sdf*/) + void GazeboRosLinkAttacher::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf) { + this->node = gazebo_ros::Node::Get(_sdf); + // Make sure the ROS node for Gazebo has already been initialized - if (!ros::isInitialized()) + if (!rclcpp::ok()) { - ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. " + RCLCPP_FATAL_STREAM(node->get_logger(), "A ROS node for Gazebo has not been initialized, unable to load plugin. " << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)"); return; } this->world = _world; this->physics = this->world->Physics(); - this->attach_service_ = this->nh_.advertiseService("attach", &GazeboRosLinkAttacher::attach_callback, this); - ROS_INFO_STREAM("Attach service at: " << this->nh_.resolveName("attach")); - this->detach_service_ = this->nh_.advertiseService("detach", &GazeboRosLinkAttacher::detach_callback, this); - ROS_INFO_STREAM("Detach service at: " << this->nh_.resolveName("detach")); - ROS_INFO("Link attacher node initialized."); + this->attach_service_ = this->node->create_service("attach", + std::bind(&GazeboRosLinkAttacher::attach_callback, this, std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO_STREAM(node->get_logger(), "Attach service created"); + this->detach_service_ = this->node->create_service("detach", + std::bind(&GazeboRosLinkAttacher::detach_callback, this, std::placeholders::_1, std::placeholders::_2)); + RCLCPP_INFO_STREAM(node->get_logger(), "Detach service created"); + RCLCPP_INFO(node->get_logger(), "Link attacher node initialized."); } bool GazeboRosLinkAttacher::attach(std::string model1, std::string link1, @@ -52,73 +51,73 @@ namespace gazebo // gazebo hangs/crashes fixedJoint j; if(this->getJoint(model1, link1, model2, link2, j)){ - ROS_INFO_STREAM("Joint already existed, reusing it."); + RCLCPP_INFO_STREAM(node->get_logger(), "Joint already existed, reusing it."); j.joint->Attach(j.l1, j.l2); return true; } else{ - ROS_INFO_STREAM("Creating new joint."); + RCLCPP_INFO_STREAM(node->get_logger(), "Creating new joint."); } j.model1 = model1; j.link1 = link1; j.model2 = model2; j.link2 = link2; - ROS_DEBUG_STREAM("Getting BasePtr of " << model1); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Getting BasePtr of " << model1); physics::BasePtr b1 = this->world->ModelByName(model1); if (b1 == NULL){ - ROS_ERROR_STREAM(model1 << " model was not found"); + RCLCPP_ERROR_STREAM(node->get_logger(), model1 << " model was not found"); return false; } - ROS_DEBUG_STREAM("Getting BasePtr of " << model2); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Getting BasePtr of " << model2); physics::BasePtr b2 = this->world->ModelByName(model2); if (b2 == NULL){ - ROS_ERROR_STREAM(model2 << " model was not found"); + RCLCPP_ERROR_STREAM(node->get_logger(), model2 << " model was not found"); return false; } - ROS_DEBUG_STREAM("Casting into ModelPtr"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Casting into ModelPtr"); physics::ModelPtr m1(dynamic_cast(b1.get())); j.m1 = m1; physics::ModelPtr m2(dynamic_cast(b2.get())); j.m2 = m2; - ROS_DEBUG_STREAM("Getting link: '" << link1 << "' from model: '" << model1 << "'"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Getting link: '" << link1 << "' from model: '" << model1 << "'"); physics::LinkPtr l1 = m1->GetLink(link1); if (l1 == NULL){ - ROS_ERROR_STREAM(link1 << " link was not found"); + RCLCPP_ERROR_STREAM(node->get_logger(), link1 << " link was not found"); return false; } if (l1->GetInertial() == NULL){ - ROS_ERROR_STREAM("link1 inertia is NULL!"); + RCLCPP_ERROR_STREAM(node->get_logger(), "link1 inertia is NULL!"); } else - ROS_DEBUG_STREAM("link1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->Mass()); + RCLCPP_DEBUG_STREAM(node->get_logger(), "link1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->Mass()); j.l1 = l1; - ROS_DEBUG_STREAM("Getting link: '" << link2 << "' from model: '" << model2 << "'"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Getting link: '" << link2 << "' from model: '" << model2 << "'"); physics::LinkPtr l2 = m2->GetLink(link2); if (l2 == NULL){ - ROS_ERROR_STREAM(link2 << " link was not found"); + RCLCPP_ERROR_STREAM(node->get_logger(), link2 << " link was not found"); return false; } if (l2->GetInertial() == NULL){ - ROS_ERROR_STREAM("link2 inertia is NULL!"); + RCLCPP_ERROR_STREAM(node->get_logger(), "link2 inertia is NULL!"); } else - ROS_DEBUG_STREAM("link2 inertia is not NULL, for example, mass is: " << l2->GetInertial()->Mass()); + RCLCPP_DEBUG_STREAM(node->get_logger(), "link2 inertia is not NULL, for example, mass is: " << l2->GetInertial()->Mass()); j.l2 = l2; - ROS_DEBUG_STREAM("Links are: " << l1->GetName() << " and " << l2->GetName()); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Links are: " << l1->GetName() << " and " << l2->GetName()); - ROS_DEBUG_STREAM("Creating revolute joint on model: '" << model1 << "'"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Creating revolute joint on model: '" << model1 << "'"); j.joint = this->physics->CreateJoint("revolute", m1); this->joints.push_back(j); - ROS_DEBUG_STREAM("Attach"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Attach"); j.joint->Attach(l1, l2); - ROS_DEBUG_STREAM("Loading links"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Loading links"); j.joint->Load(l1, l2, ignition::math::Pose3d()); - ROS_DEBUG_STREAM("SetModel"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "SetModel"); j.joint->SetModel(m2); /* * If SetModel is not done we get: @@ -134,13 +133,13 @@ namespace gazebo /tmp/buildd/gazebo2-2.2.3/gazebo/physics/ode/ODELink.cc(183): Inertial pointer is NULL */ - ROS_DEBUG_STREAM("SetHightstop"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "SetHightstop"); j.joint->SetUpperLimit(0, 0); - ROS_DEBUG_STREAM("SetLowStop"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "SetLowStop"); j.joint->SetLowerLimit(0, 0); - ROS_DEBUG_STREAM("Init"); + RCLCPP_DEBUG_STREAM(node->get_logger(), "Init"); j.joint->Init(); - ROS_INFO_STREAM("Attach finished."); + RCLCPP_INFO_STREAM(node->get_logger(), "Attach finished."); return true; } @@ -174,38 +173,38 @@ namespace gazebo } - bool GazeboRosLinkAttacher::attach_callback(gazebo_ros_link_attacher::Attach::Request &req, - gazebo_ros_link_attacher::Attach::Response &res) + bool GazeboRosLinkAttacher::attach_callback(const std::shared_ptr req, + std::shared_ptr res) { - ROS_INFO_STREAM("Received request to attach model: '" << req.model_name_1 - << "' using link: '" << req.link_name_1 << "' with model: '" - << req.model_name_2 << "' using link: '" << req.link_name_2 << "'"); - if (! this->attach(req.model_name_1, req.link_name_1, - req.model_name_2, req.link_name_2)){ - ROS_ERROR_STREAM("Could not make the attach."); - res.ok = false; + RCLCPP_INFO_STREAM(node->get_logger(), "Received request to attach model: '" << req->model_name_1 + << "' using link: '" << req->link_name_1 << "' with model: '" + << req->model_name_2 << "' using link: '" << req->link_name_2 << "'"); + if (! this->attach(req->model_name_1, req->link_name_1, + req->model_name_2, req->link_name_2)){ + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not make the attach."); + res->ok = false; } else{ - ROS_INFO_STREAM("Attach was succesful"); - res.ok = true; + RCLCPP_INFO_STREAM(node->get_logger(), "Attach was succesful"); + res->ok = true; } return true; } - bool GazeboRosLinkAttacher::detach_callback(gazebo_ros_link_attacher::Attach::Request &req, - gazebo_ros_link_attacher::Attach::Response &res){ - ROS_INFO_STREAM("Received request to detach model: '" << req.model_name_1 - << "' using link: '" << req.link_name_1 << "' with model: '" - << req.model_name_2 << "' using link: '" << req.link_name_2 << "'"); - if (! this->detach(req.model_name_1, req.link_name_1, - req.model_name_2, req.link_name_2)){ - ROS_ERROR_STREAM("Could not make the detach."); - res.ok = false; + bool GazeboRosLinkAttacher::detach_callback(const std::shared_ptr req, + std::shared_ptr res){ + RCLCPP_INFO_STREAM(node->get_logger(), "Received request to detach model: '" << req->model_name_1 + << "' using link: '" << req->link_name_1 << "' with model: '" + << req->model_name_2 << "' using link: '" << req->link_name_2 << "'"); + if (! this->detach(req->model_name_1, req->link_name_1, + req->model_name_2, req->link_name_2)){ + RCLCPP_ERROR_STREAM(node->get_logger(), "Could not make the detach."); + res->ok = false; } else{ - ROS_INFO_STREAM("Detach was succesful"); - res.ok = true; + RCLCPP_INFO_STREAM(node->get_logger(), "Detach was succesful"); + res->ok = true; } return true; } diff --git a/worlds/test_attacher.world b/worlds/test_attacher.world index 50a7dad..3649065 100644 --- a/worlds/test_attacher.world +++ b/worlds/test_attacher.world @@ -1,8 +1,14 @@ - + + + + /gazebo + + + - + model://sun @@ -11,4 +17,4 @@ model://ground_plane - + \ No newline at end of file