ROS2: Spawn robot with namespace in gazebo

asked 2019-07-30 04:34:37 -0500

madmax gravatar image

updated 2019-07-30 04:36:29 -0500

I am trying to add a robot to a gazebo simulation but when I want to use a namespace, gazebo throws an exception..

Error [Element.cc:702] Missing element description for [ros]

For spawning I use this script, which was posted here some time ago:

def main(args=None):
    rclpy.init(args=args)
    node = rclpy.create_node('minimal_client')
    cli = node.create_client(SpawnEntity, '/spawn_entity')

    content = ""
    if sys.argv[1] is not None:
        with open(sys.argv[1], 'r') as content_file:
            content = content_file.read()

    req = SpawnEntity.Request()
    req.name = "osg_shuttle"
    req.xml = content
    req.robot_namespace = node.get_namespace()
    req.reference_frame = "world"

    node.get_logger().info('Spawning shuttle with namespace ' + node.get_namespace() + " and name " + req.name )

    while not cli.wait_for_service(timeout_sec=3.0):
        node.get_logger().info('service not available, waiting again...')

    future = cli.call_async(req)
    rclpy.spin_until_future_complete(node, future)

    if future.result() is not None:
        node.get_logger().info(
            'Result ' + str(future.result().success) + " " + future.result().status_message)
    else:
        node.get_logger().info('Service call failed %r' % (future.exception(),))

    node.destroy_node()
    rclpy.shutdown()

Script is working fine, only when I use the namespace req.robot_namespace = node.get_namespace(), gazebo is killed with this stacktrace:

Thread 45 "gzserver" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fd29f7fe700 (LWP 10063)]
0x00007fd35f6c05bd in sdf::Element::GetElementImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const () from /usr/lib/x86_64-linux-gnu/libsdformat.so.6
(gdb) backtrace 
#0  0x00007fd35f6c05bd in sdf::Element::GetElementImpl(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const () at /usr/lib/x86_64-linux-gnu/libsdformat.so.6
#1  0x00007fd35f6c06b8 in sdf::Element::HasElement(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) const () at /usr/lib/x86_64-linux-gnu/libsdformat.so.6
#2  0x00007fd32a0c036d in gazebo_ros::GazeboRosFactoryPrivate::AddNamespace(std::shared_ptr<sdf::Element> const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&) () at /opt/ros/dashing/lib/libgazebo_ros_factory.so
#3  0x00007fd32a0c1e6a in gazebo_ros::GazeboRosFactoryPrivate::SpawnEntity(std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Request_<std::allocator<void> > >, std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Response_<std::allocator<void> > >) ()
    at /opt/ros/dashing/lib/libgazebo_ros_factory.so
#4  0x00007fd32a0c6438 in std::_Function_handler<void (std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Request_<std::allocator<void> > >, std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Response_<std::allocator<void> > >), std::_Bind<void (gazebo_ros::GazeboRosFactoryPrivate::*(gazebo_ros::GazeboRosFactoryPrivate*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Request_<std::allocator<void> > >, std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Response_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Request_<std::allocator<void> > >&&, std::shared_ptr<gazebo_msgs::srv::SpawnEntity_Response_<std::allocator<void> > >&&) () at /opt/ros/dashing/lib/libgazebo_ros_factory.so
#5  0x00007fd32a0c6ef7 in rclcpp::Service<gazebo_msgs::srv::SpawnEntity>::handle_request(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<void>) () at /opt/ros/dashing/lib/libgazebo_ros_factory.so
#6  0x00007fd329de6180 in rclcpp::executor::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>) () at /opt/ros/dashing/lib/librclcpp.so
#7  0x00007fd329de6be5 in rclcpp::executor::Executor::execute_any_executable(rclcpp::executor::AnyExecutable&) () at /opt/ros/dashing/lib/librclcpp.so

Really don't know where the problem is...
Gazebo ros plugin bug or do I need to set a namespace in the xacro of the robot? I don't think so.

Log ouput of the spawner is Spawning robot with namespace /robot_1 and name osg_shuttle

edit retag flag offensive close merge delete

Comments

1

A shot in the dark, but this is possibly resolved by https://github.com/ros-simulation/gaz...

jacobperron gravatar image jacobperron  ( 2019-08-21 13:29:37 -0500 )edit