Create maliput ros node #8

agalbachicar · 2023-01-02T09:40:58Z

🎉 New feature

Part of #1

Summary

Creates a node and provides a launchfile to execute it. The node only responds to the /road_geometry service call at the moment and lacks of testing, it will be implemented in a subsequent PR.

Also, there is no explicit exec_depend on maliput_malidrive as there is actually no dependency with this example resource file (note it is not installed on purpose). We still need to define what to do with it.

Test it

Unit tests, and you can try launching it from the terminal:

$ colcon build --packages-up-to maliput_malidrive maliput_ros --event-handlers console_direct+
[...]
$ source install/setup.bash
$ ros2 launch maliput_ros maliput_ros.launch.py maliput_yaml_path:="/home/agalbachicar/maliput_ws_focal/src/ros2_maliput/maliput_ros/resources/maliput_malidrive_plugin.yml"
[INFO] [launch]: All log files can be found below /home/agalbachicar/.ros/log/2023-01-02-09-37-52-750746-5a83e2746402-54415
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [maliput_ros-1]: process started with pid [54427]
[maliput_ros-1] [INFO] [1672652272.908667953] [query_server]: MaliputQueryNode
[maliput_ros-1] [INFO] [1672652273.137092467] [query_server]: on_configure
[maliput_ros-1] [INFO] [1672652273.137155825] [query_server]: LoadMaliputQuery
[maliput_ros-1] [INFO] [1672652273.137179680] [query_server]: File path: /home/agalbachicar/maliput_ws_focal/src/ros2_maliput/maliput_ros/resources/maliput_malidrive_plugin.yml
[maliput_ros-1] [INFO] Plugin Id: maliput_malidrive was correctly loaded.
[maliput_ros-1] [INFO] Number of plugins loaded: 1
[maliput_ros-1] [INFO] RoadGeometry loaded successfully after iteration [0] using:
[maliput_ros-1]         |__ linear_tolerance = 0.001
[maliput_ros-1]         |__ angular_tolerance = 0.001
[maliput_ros-1]         |__ scale_length = 1
[maliput_ros-1] [INFO] [1672652273.230751628] [query_server]: InitializeAllServices
[INFO] [launch.user]: node 'query_server' reached the 'inactive' state, 'activating'.
[maliput_ros-1] [INFO] [1672652273.233952199] [query_server]: on_activate
[INFO] [launch.user]: node 'query_server' reached the 'active' state, launching 'listener'.

From another terminal, you can try:

$ ros2 service call /road_geometry maliput_ros_interfaces/srv/RoadGeometry {}[3/1801]
>                                                                                                                                
waiting for service to become available...                                                                                       
requester: making request: maliput_ros_interfaces.srv.RoadGeometry_Request()                                                     

response:
maliput_ros_interfaces.srv.RoadGeometry_Response(road_geometry=maliput_ros_interfaces.msg.RoadGeometry(id=maliput_ros_interfaces.msg.RoadGeometryId(id='t_shape_road'), junction_ids=[maliput_ros_interfaces.msg.JunctionId(id='0_0'), maliput_ros_interfaces.msg.JunctionId(id='1_0'), maliput_ros_interfaces.msg.JunctionId(id='2_0'), maliput_ros_interfaces.msg.JunctionId(id='3')], branch_point_ids=[maliput_ros_interfaces.msg.BranchPointId(id='1'), maliput_ros_interfaces.msg.BranchPointId(id='2'), maliput_ros_interfaces.msg.BranchPointId(id='3'), maliput_ros_interfaces.msg.BranchPointId(id='4'), maliput_ros_interfaces.msg.BranchPointId(id='5'), maliput_ros_interfaces.msg.BranchPointId(id='6'), maliput_ros_interfaces.msg.BranchPointId(id='7'), maliput_ros_interfaces.msg.BranchPointId(id='8'), maliput_ros_interfaces.msg.BranchPointId(id='9'), maliput_ros_interfaces.msg.BranchPointId(id='10'), maliput_ros_interfaces.msg.BranchPointId(id='11'), maliput_ros_interfaces.msg.BranchPointId(id='12')], linear_tolerance=0.001, angular_t
olerance=0.001, scale_length=1.0, inertial_to_backend_frame_translation=geometry_msgs.msg.Vector3(x=0.0, y=0.0, z=0.0)))

Checklist

Signed all commits for DCO
Added tests
Added example and/or tutorial
Updated documentation (as needed)
Updated migration guide (as needed)
Consider updating Python bindings (if it affects the public API)

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

- Creates MaliputQueryNode as a ROS2 proxy to make RoadNetwork queries. - Provides a sample configuration file use maliput_malidrive. Signed-off-by: Agustin Alba Chicar <ag.albachicar@gmail.com>

francocipollone

Great Job! Happy to see a ros node with maliput! I left some comments, ptal.

francocipollone · 2023-01-02T12:44:00Z

maliput_ros/CMakeLists.txt

+ament_export_include_directories(include)
+
+install(DIRECTORY
+  launch


are you missing resources as well?

francocipollone · 2023-01-02T12:45:53Z

maliput_ros/include/maliput_ros/ros/maliput_query_node.h

+/// MaliputQueryNode is a LifecycleNode that serves as proxy to a maliput::api::RoadNetwork
+/// set of queries.
+///
+/// It defines the following parameters:


nit:

Suggested change

/// It defines the following parameters:

/// It defines the following ros parameters:

I would also comment here what is the expected format of the yaml file. Maybe linking to maliput_ros::utils::YamlParser if you don't want to copy-paste the example because of duplicating docs.

francocipollone · 2023-01-02T12:57:41Z

maliput_ros/include/maliput_ros/ros/maliput_query_node.h

+  MaliputQueryNode(const std::string& node_name, const std::string& namespace_,
+                   const rclcpp::NodeOptions& options = rclcpp::NodeOptions());


I think that with a small change we can evolve it to be a component, we simply need to provide the constructor with the firm MyClass(const rclcpp::NodeOptions& options)

Suggested change

MaliputQueryNode(const std::string& node_name, const std::string& namespace_,

const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

MaliputQueryNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

For setting a different node name or different namespace the remapping could be used:

Name remapping

Also Remapping - Design

Note: I could push a commit with the suggestion if you want.

If you don't mind, I'd prefer to push it to a follow up iteration. I just left one constructor with default values for the namespace to consider the other case. That way, we can decide how to configure the lifecycle node constructor which demands a node name.
BTW, I haven't seen examples of composition with lifecycle managed nodes.

Sure no worries, it is just enhancement, I will create the issue(#9) and I could try to tackle this later on.

BTW, I haven't seen examples of composition with lifecycle managed nodes.

There are no ros2 examples in the docs but you can find code out there with this approach like this one

francocipollone · 2023-01-02T13:21:40Z

maliput_ros/include/maliput_ros/ros/maliput_query.h

+  explicit MaliputQuery(std::unique_ptr<maliput::api::RoadNetwork> road_network)
+      : road_network_(std::move(road_network)) {


QQ: Is there a reason why MaliputQuery is the owner of the RoadNetwork?
I am ok with this, at the moment there are no other applications

Simply because there is no other one requiring to hold the ownership of it. The ownership lifecycle will be tied as well so even though you are right, there is no explicit ownership relationship, it sounded OK to me leave to the MaliputQuery the responsibility to own that memory and simplify the management at the node level.

francocipollone · 2023-01-02T13:25:07Z

maliput_ros/launch/maliput_ros.launch.py

+            target_lifecycle_node=maliput_query_server_node, goal_state='active',
+            entities=[
+                launch.actions.LogInfo(
+                    msg="node 'query_server' reached the 'active' state, launching 'listener'."),


Check the msg: "... launching listener" seems to be part of an example.

francocipollone · 2023-01-02T13:25:41Z

maliput_ros/launch/maliput_ros.launch.py

+        )
+    )
+
+    # Make the talker node take the 'configure' transition.


Check: "# Make the talker node take the 'configure' transition."

francocipollone · 2023-01-02T13:30:19Z

maliput_ros/launch/maliput_ros.launch.py

+
+    # The query server node.
+    maliput_query_server_node = launch_ros.actions.LifecycleNode(
+        package='maliput_ros', executable='maliput_ros', name='query_server', output='screen',


For the name, what about maliput_query_server instead ? So the node name is more self-describable.

Also, we could provide here a namespace so the services are advertised under the node name for example:

So the service: Like the /road_geometry and other services when provided are better organized.

francocipollone · 2023-01-02T13:34:56Z

maliput_ros/src/maliput_ros/ros/maliput_query_node.cc

+
+MaliputQueryNode::MaliputQueryNode(const std::string& node_name, const std::string& namespace_,
+                                   const rclcpp::NodeOptions& options)
+    : rclcpp_lifecycle::LifecycleNode(node_name, namespace_, options), is_active_(false) {


As the LifeCycleNode already advertise several services for the state transition and stuff, maybe it makes sense to hide those services under a namespace so those services are left at <node_name>/lifecycle/ , while the services that this node provides are under <node_name>.

Nowadays:

franco@9c2d60d201e3:~/maliput_ws_focal$ ros2 service list /launch_ros_2177/describe_parameters /launch_ros_2177/get_parameter_types /launch_ros_2177/get_parameters /launch_ros_2177/list_parameters /launch_ros_2177/set_parameters /launch_ros_2177/set_parameters_atomically /query_server/change_state /query_server/describe_parameters /query_server/get_available_states /query_server/get_available_transitions /query_server/get_parameter_types /query_server/get_parameters /query_server/get_state /query_server/get_transition_graph /query_server/list_parameters /query_server/set_parameters /query_server/set_parameters_atomically /road_geometry

francocipollone · 2023-01-02T13:40:12Z

maliput_ros/src/maliput_ros/ros/maliput_query_server.cc

+int Main(int argc, char* argv[]) {
+  ForceStdoutFlush();
+  rclcpp::init(argc, argv);
+  rclcpp::executors::SingleThreadedExecutor executor;


Documenting for future iterations:

Not sure if we are going to be able to handle queries in parallel. If so, at some point we will need to use a MultiTthreadedExecutor: See Executors

francocipollone · 2023-01-02T13:49:16Z

maliput_ros/resources/maliput_malidrive_plugin.yml

+  backend: "maliput_malidrive"
+  parameters:
+    road_geometry_id: "t_shape_road"
+    opendrive_file: "/home/agalbachicar/maliput_ws_focal/install/maliput_malidrive/share/maliput_malidrive/resources/odr/TShapeRoad.xodr"


If it is just an example note that you can use here:

Suggested change

opendrive_file: "/home/agalbachicar/maliput_ws_focal/install/maliput_malidrive/share/maliput_malidrive/resources/odr/TShapeRoad.xodr"

opendrive_file: "/opt/ros/foxy/share/maliput_malidrive/resources/odr/TShapeRoad.xodr"

(sudo apt install ros-foxy-maliput-malidrive)

francocipollone · 2023-01-02T14:02:13Z

maliput_ros/launch/maliput_ros.launch.py

+    emit_event_to_request_that_query_server_does_configure_transition = launch.actions.EmitEvent(
+        event=launch_ros.events.lifecycle.ChangeState(
+            lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node),
+            transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,
+        )
+    )


What about doing on state instead of just emitting?

Suggested change

emit_event_to_request_that_query_server_does_configure_transition = launch.actions.EmitEvent(

event=launch_ros.events.lifecycle.ChangeState(

lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node),

transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,

)

)

register_event_handler_for_query_server_reaches_unconfigured_state = launch.actions.RegisterEventHandler(

launch_ros.event_handlers.OnStateTransition(

target_lifecycle_node=maliput_query_server_node, goal_state='unconfigured',

entities=[

launch.actions.LogInfo(

msg="node 'query_server' reached the 'unconfigured' state, 'configuring'."),

launch.actions.EmitEvent(event=launch_ros.events.lifecycle.ChangeState(

lifecycle_node_matcher=launch.events.matches_action(maliput_query_server_node),

transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE,

)),

],

)

This does not work. It does not trigger the initialization.

francocipollone · 2023-01-02T14:03:27Z

maliput_ros/launch/maliput_ros.launch.py

+        register_event_handler_for_query_server_reaches_inactive_state,
+        register_event_handler_for_query_server_reaches_active_state,


Maybe the stuff that is related to the lifecycle node we could move it to a launch file that is included here. That launchfile could live at maliput_ros/launch/include/lifecycle_config.launch.py

Pushed to a future iteration. #11

Signed-off-by: Agustin Alba Chicar <ag.albachicar@gmail.com>

francocipollone

LGTM!

agalbachicar · 2023-01-09T12:23:19Z

CI is failing due to the same that we saw in #10 . It is pep257 ament test failing due to the generated files.
I cannot reproduce it locally with my current environment. If you agree, I can try to fix it in #10 all together.

francocipollone · 2023-01-09T12:57:50Z

CI is failing due to the same that we saw in #10 . It is pep257 ament test failing due to the generated files. I cannot reproduce it locally with my current environment. If you agree, I can try to fix it in #10 all together.

Sure, go for it

Creates a base package to execute queries.

39ac9ea

- Creates MaliputQueryNode as a ROS2 proxy to make RoadNetwork queries. - Provides a sample configuration file use maliput_malidrive. Signed-off-by: Agustin Alba Chicar <ag.albachicar@gmail.com>

agalbachicar requested a review from francocipollone January 2, 2023 09:41

agalbachicar mentioned this pull request Jan 2, 2023

ROS2 interface for the RoadNetwork #1

Open

22 tasks

francocipollone requested changes Jan 2, 2023

View reviewed changes

francocipollone reviewed Jan 2, 2023

View reviewed changes

francocipollone mentioned this pull request Jan 3, 2023

Evolve node to a component #9

Open

agalbachicar mentioned this pull request Jan 5, 2023

Add tests to ROS node #10

Merged

6 tasks

Addresses comments.

7941506

Signed-off-by: Agustin Alba Chicar <ag.albachicar@gmail.com>

agalbachicar mentioned this pull request Jan 9, 2023

Improve launch configuration #11

Open

agalbachicar requested a review from francocipollone January 9, 2023 09:17

francocipollone approved these changes Jan 9, 2023

View reviewed changes

agalbachicar merged commit 7e6d5aa into main Jan 9, 2023

agalbachicar deleted the agalbachicar/#1_create_maliput_ros_node branch January 9, 2023 13:27

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Create maliput ros node #8

Create maliput ros node #8

agalbachicar commented Jan 2, 2023

francocipollone left a comment

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

francocipollone Jan 2, 2023 •

edited

Loading

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023 •

edited

Loading

agalbachicar Jan 3, 2023

francocipollone Jan 3, 2023 •

edited

Loading

francocipollone Jan 2, 2023

agalbachicar Jan 3, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone Jan 2, 2023

agalbachicar Jan 9, 2023

francocipollone left a comment

agalbachicar commented Jan 9, 2023

francocipollone commented Jan 9, 2023

	/// It defines the following parameters:
	/// It defines the following ros parameters:

		MaliputQueryNode(const std::string& node_name, const std::string& namespace_,
		const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

	MaliputQueryNode(const std::string& node_name, const std::string& namespace_,
	const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
	MaliputQueryNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());

		explicit MaliputQuery(std::unique_ptr<maliput::api::RoadNetwork> road_network)
		: road_network_(std::move(road_network)) {

	opendrive_file: "/home/agalbachicar/maliput_ws_focal/install/maliput_malidrive/share/maliput_malidrive/resources/odr/TShapeRoad.xodr"
	opendrive_file: "/opt/ros/foxy/share/maliput_malidrive/resources/odr/TShapeRoad.xodr"

		register_event_handler_for_query_server_reaches_inactive_state,
		register_event_handler_for_query_server_reaches_active_state,

Create maliput ros node #8

Create maliput ros node #8

Conversation

agalbachicar commented Jan 2, 2023

🎉 New feature

Summary

Test it

Checklist

francocipollone left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francocipollone Jan 2, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francocipollone Jan 2, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francocipollone Jan 3, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francocipollone left a comment

Choose a reason for hiding this comment

agalbachicar commented Jan 9, 2023

francocipollone commented Jan 9, 2023

francocipollone Jan 2, 2023 •

edited

Loading

francocipollone Jan 2, 2023 •

edited

Loading

francocipollone Jan 3, 2023 •

edited

Loading