diff --git a/cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml b/cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml deleted file mode 100644 index f376345e..00000000 --- a/cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml +++ /dev/null @@ -1,66 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/cabot_bt/behavior_trees/navigation.xml b/cabot_bt/behavior_trees/navigation.xml deleted file mode 100644 index 44d37534..00000000 --- a/cabot_bt/behavior_trees/navigation.xml +++ /dev/null @@ -1,90 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/cabot_navigation2/include/cabot_navigation2/cabot_planner_param.hpp b/cabot_navigation2/include/cabot_navigation2/cabot_planner_param.hpp index 66c87d72..d65c14e5 100644 --- a/cabot_navigation2/include/cabot_navigation2/cabot_planner_param.hpp +++ b/cabot_navigation2/include/cabot_navigation2/cabot_planner_param.hpp @@ -63,6 +63,7 @@ struct CaBotPlannerOptions int kdtree_max_results = 50; int min_iteration_count = 500; int max_iteration_count = 1000; + bool ignore_obstacles = false; // private params float iteration_scale_min = 0.0001; diff --git a/cabot_navigation2/plugins/cabot_planner.cpp b/cabot_navigation2/plugins/cabot_planner.cpp index d1547b32..ad5626d3 100644 --- a/cabot_navigation2/plugins/cabot_planner.cpp +++ b/cabot_navigation2/plugins/cabot_planner.cpp @@ -174,6 +174,11 @@ void CaBotPlanner::configure( rclcpp::ParameterValue(defaultValue.max_iteration_count)); node->get_parameter(name + ".max_iteration_count", options_.max_iteration_count); + declare_parameter_if_not_declared( + node, name + ".ignore_obstacles", + rclcpp::ParameterValue(defaultValue.ignore_obstacles)); + node->get_parameter(name + ".ignore_obstacles", options_.ignore_obstacles); + declare_parameter_if_not_declared(node, name + ".static_layer_name", rclcpp::ParameterValue("static_layer")); node->get_parameter(name + ".static_layer_name", static_layer_name_); @@ -410,6 +415,9 @@ rcl_interfaces::msg::SetParametersResult CaBotPlanner::param_set_callback(const if (param.get_name() == name_ + ".max_iteration_count") { options_.max_iteration_count = param.as_int(); } + if (param.get_name() == name_ + ".ignore_obstacles") { + options_.ignore_obstacles = param.as_bool(); + } // private if (param.get_name() == name_ + ".private.iteration_scale_min") { @@ -532,8 +540,14 @@ nav_msgs::msg::Path CaBotPlanner::createPlan(CaBotPlannerParam & param) int total_count = 0; DetourMode modes[3] = {DetourMode::RIGHT, DetourMode::LEFT, DetourMode::IGNORE}; + int i = 0; + if (param.options.ignore_obstacles) { + plans[0].okay = false; + plans[1].okay = false; + i = 2; + } rclcpp::Rate r(1); - for (int i = 0; i < 3; i++) { + for (; i < 3; i++) { CaBotPlan & plan = plans[i]; plan.detour_mode = modes[i]; diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py index df1288c2..d5837e92 100644 --- a/cabot_ui/cabot_ui/geojson.py +++ b/cabot_ui/cabot_ui/geojson.py @@ -28,6 +28,7 @@ # -*- coding: utf-8 -*- import sys import traceback +import enum import copy import math import json @@ -381,6 +382,20 @@ def copy(self): return copy.deepcopy(self) +class NavigationMode(enum.Enum): + Standard = 0 + Narrow = 1 + Tight = 2 + + @classmethod + def get_mode(cls, width): + if width < 1.0: + return NavigationMode.Tight + if width < 1.2: + return NavigationMode.Narrow + return NavigationMode.Standard + + class Link(Object): """Link class""" ROUTE_TYPE_WALKWAY = 1 @@ -391,8 +406,6 @@ class Link(Object): ROUTE_TYPE_STAIRS = 6 ROUTE_TYPE_SLOPE = 7 ROUTE_TYPE_UNKNOWN = 99 - ROUTE_NARROW_PATH_THRESHOLD = 1.2 - ROUTE_NARROW_ANNOUNCE_THRESHOLD = 1.0 @classmethod def marshal(cls, dic): @@ -445,14 +458,8 @@ def is_leaf(self): return self.start_node.is_leaf or self.end_node.is_leaf @property - def is_narrow(self): - """wheather this links is narrow or not""" - return self.properties.hulop_road_width < Link.ROUTE_NARROW_PATH_THRESHOLD - - @property - def need_narrow_announce(self): - """wheather this links is narrow or not""" - return self.properties.hulop_road_width < Link.ROUTE_NARROW_ANNOUNCE_THRESHOLD + def navigation_mode(self): + return NavigationMode.get_mode(self.properties.hulop_road_width) @property def length(self): diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index a5b82a26..3df7a8c6 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -20,8 +20,10 @@ import math import inspect +from itertools import groupby import numpy import time +import traceback from cabot_ui import geoutil, geojson from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil @@ -78,6 +80,9 @@ def please_follow_behind(self): def please_return_position(self): CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") + def change_parameters(self, params): + CaBotRclpyUtil.error(F"{inspect.currentframe().f_code.co_name} is not implemented") + def make_goals(delegate, groute, anchor, yaw=None): """ @@ -107,14 +112,13 @@ def make_goals(delegate, groute, anchor, yaw=None): goals = [] route_objs = [] index = 1 - narrow = groute[1].is_narrow while index < len(groute): link_or_node = groute[index] - route_objs.append(link_or_node) index += 1 if not isinstance(link_or_node, geojson.RouteLink): continue + route_objs.append(link_or_node) link = link_or_node # Handle Door POIs @@ -271,23 +275,10 @@ def make_goals(delegate, groute, anchor, yaw=None): # CaBotRclPyUtil.info(goals[-1]) route_objs = [] - if link.is_narrow and not narrow: - narrow = True - goals.append(NavGoal(delegate, route_objs[:-1], anchor)) - route_objs = [link] - - if (not link.is_narrow) and narrow: - narrow = False - goals.append(NarrowGoal(delegate, route_objs[:-1], anchor)) - route_objs = [link] - # TODO: escalator + if len(route_objs) > 0: - if narrow: - narrow = False - goals.append(NarrowGoal(delegate, route_objs, anchor, is_last=True)) - else: - goals.append(NavGoal(delegate, route_objs, anchor, is_last=True)) + goals.append(NavGoal(delegate, route_objs, anchor, is_last=True)) if yaw is not None: goal_node = groute[-1] # should be Node @@ -389,7 +380,7 @@ def convert(g, a=anchor): path.poses[-1].pose.position.y = last_pose.position.y CaBotRclpyUtil.info(F"path {path}") - return (path, path.poses[-1] if len(path.poses) > 0 else None) + return (path, path.poses[-1] if len(path.poses) > 0 else None, navcog_route) def estimate_next_goal(goals, current_pose, current_floor): @@ -419,6 +410,7 @@ def __init__(self, delegate, **kwargs): self._current_statement = None self.global_map_name = self.delegate.global_map_name() self._handles = [] + self._speed_limit = None def reset(self): self._is_completed = False @@ -444,6 +436,10 @@ def is_completed(self): def is_canceled(self): return self._is_canceled + @property + def speed_limit(self): + return self._speed_limit + def exit(self): self.delegate.exit_goal(self) @@ -513,7 +509,7 @@ class NavGoal(Goal): link and node:(R) -----o----- link and node:(R) -----o-----o """ - DEFAULT_BT_XML = "package://cabot_bt/behavior_trees/navigate_w_replanning_and_recovery.xml" + DEFAULT_BT_XML = "package://cabot_bt/behavior_trees/cabot_navigate.xml" NEIGHBOR_THRESHOLD = 1.0 def __init__(self, delegate, navcog_route, anchor, target_poi=None, set_back=(0, 0), **kwargs): @@ -529,9 +525,17 @@ def __init__(self, delegate, navcog_route, anchor, target_poi=None, set_back=(0, self.global_map_name = delegate.global_map_name() self.navcog_route = navcog_route self.anchor = anchor - (self.ros_path, last_pose) = create_ros_path(self.navcog_route, self.anchor, self.global_map_name, target_poi=target_poi, set_back=set_back) + separated_route = [list(group) for _, group in groupby(navcog_route, key=lambda x: x.navigation_mode)] + self.navcog_routes = [create_ros_path(route, + self.anchor, + self.global_map_name, + target_poi=target_poi, + set_back=set_back) for route in separated_route] + last_pose = self.navcog_routes[-1][1] self.pois = self._extract_pois() self.handle = None + self.mode = None + self.route_index = 0 super(NavGoal, self).__init__(delegate, angle=180, floor=navcog_route[-1].floor, pose_msg=last_pose, **kwargs) def _extract_pois(self): @@ -545,8 +549,237 @@ def _extract_pois(self): temp.extend(item.pois) return temp + @util.setInterval(0.2, times=1) + def set_speed_limit(self, limit, func, timeout, interval): + CaBotRclpyUtil.info(f"set speed limit = {limit}, timeout = {timeout}") + self._speed_limit = limit + if limit > 0: + return self.set_speed_limit(limit-interval, func, timeout, interval) + if func: + func() + if timeout > 0: + return self.set_speed_limit(0.0, None, timeout-interval, interval) + self._speed_limit = None + + def get_parameters_for(self, mode): + if mode == geojson.NavigationMode.Standard: + return [{ + "node_name": "/planner_server", + "param_name": "CaBot.path_adjusted_center", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_adjusted_minimum_path_width", + "param_value": 0.5 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_min_width", + "param_value": 0.5 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_width", + "param_value": 2.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.min_iteration_count", + "param_value": 500 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.max_iteration_count", + "param_value": 1000 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.ignore_obstacles", + "param_value": False + }, { + "node_name": "/footprint_publisher", + "param_name": "footprint_mode", + "param_value": 0 + }, { + "node_name": "/controller_server", + "param_name": "FollowPath.max_vel_x", + "param_value": 1.0 + }, { + "node_name": "/controller_server", + "param_name": "FollowPath.sim_time", + "param_value": 1.7 + }, { + "node_name": "/controller_server", + "param_name": "cabot_goal_checker.xy_goal_tolerance", + "param_value": 0.5 + }, { + "node_name": "/global_costmap/global_costmap", + "param_name": "people_obstacle_layer.people_enabled", + "param_value": True + }, { + "node_name": "/global_costmap/global_costmap", + "param_name": "inflation_layer.inflation_radius", + "param_value": 0.75 + }, { + "node_name": "/local_costmap/local_costmap", + "param_name": "inflation_layer.inflation_radius", + "param_value": 0.75 + }, { + "node_name": "/footprint_publisher", + "param_name": "footprint_mode", + "param_value": 0 + }, { + "node_name": "/cabot/lidar_speed_control_node", + "param_name": "min_distance", + "param_value": 1.0 + }] + + if mode == geojson.NavigationMode.Narrow: + return [{ + "node_name": "/planner_server", + "param_name": "CaBot.path_adjusted_center", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_adjusted_minimum_path_width", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_min_width", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_width", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.min_iteration_count", + "param_value": 5 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.max_iteration_count", + "param_value": 10 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.ignore_obstacles", + "param_value": True + }, { + "node_name": "/footprint_publisher", + "param_name": "footprint_mode", + "param_value": 0 + }, { + "node_name": "/controller_server", + "param_name": "FollowPath.max_vel_x", + "param_value": 0.5 + }, { + "node_name": "/controller_server", + "param_name": "FollowPath.sim_time", + "param_value": 0.5 + }, { + "node_name": "/controller_server", + "param_name": "cabot_goal_checker.xy_goal_tolerance", + "param_value": 0.1 + }, { + "node_name": "/global_costmap/global_costmap", + "param_name": "people_obstacle_layer.people_enabled", + "param_value": False + }, { + "node_name": "/global_costmap/global_costmap", + "param_name": "inflation_layer.inflation_radius", + "param_value": 0.45 + }, { + "node_name": "/local_costmap/local_costmap", + "param_name": "inflation_layer.inflation_radius", + "param_value": 0.45 + }, { + "node_name": "/footprint_publisher", + "param_name": "footprint_mode", + "param_value": 3 + }, { + "node_name": "/cabot/lidar_speed_control_node", + "param_name": "min_distance", + "param_value": 0.25 + }] + if mode == geojson.NavigationMode.Tight: + return [{ + "node_name": "/planner_server", + "param_name": "CaBot.path_adjusted_center", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_adjusted_minimum_path_width", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_min_width", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.path_width", + "param_value": 0.0 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.min_iteration_count", + "param_value": 5 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.max_iteration_count", + "param_value": 10 + }, { + "node_name": "/planner_server", + "param_name": "CaBot.ignore_obstacles", + "param_value": True + }, { + "node_name": "/footprint_publisher", + "param_name": "footprint_mode", + "param_value": 0 + }, { + "node_name": "/controller_server", + "param_name": "FollowPath.max_vel_x", + "param_value": 0.5 + }, { + "node_name": "/controller_server", + "param_name": "FollowPath.sim_time", + "param_value": 0.5 + }, { + "node_name": "/controller_server", + "param_name": "cabot_goal_checker.xy_goal_tolerance", + "param_value": 0.1 + }, { + "node_name": "/global_costmap/global_costmap", + "param_name": "people_obstacle_layer.people_enabled", + "param_value": False + }, { + "node_name": "/global_costmap/global_costmap", + "param_name": "inflation_layer.inflation_radius", + "param_value": 0.25 + }, { + "node_name": "/local_costmap/local_costmap", + "param_name": "inflation_layer.inflation_radius", + "param_value": 0.25 + }, { + "node_name": "/footprint_publisher", + "param_name": "footprint_mode", + "param_value": 1 + }, { + "node_name": "/cabot/lidar_speed_control_node", + "param_name": "min_distance", + "param_value": 0.25 + }] + + def check_mode(self): + new_mode = self.navcog_routes[self.route_index][2][0].navigation_mode + CaBotRclpyUtil.info(F"NavGoal.check_mode {self.mode}, {new_mode}") + if self.mode == new_mode: + return + if new_mode == geojson.NavigationMode.Tight: + self.set_speed_limit(1.0, self.delegate.please_follow_behind, 2.5, 0.1) + if self.mode == geojson.NavigationMode.Tight: + self.set_speed_limit(0.5, self.delegate.please_return_position, 2.5, 0.1) + self.mode = new_mode + + def callback(*arguments): + CaBotRclpyUtil.info(f"change_parameters: {arguments}") + self.delegate.change_parameters(self.get_parameters_for(new_mode), callback) + def enter(self): CaBotRclpyUtil.info("NavGoal enter") + self.check_mode() # reset social distance setting if necessary if self.delegate.initial_social_distance is not None and self.delegate.current_social_distance is not None \ and (self.delegate.initial_social_distance.x != self.delegate.current_social_distance.x or @@ -556,7 +789,7 @@ def enter(self): CaBotRclpyUtil.info("NavGoal set social distance") # publish navcog path - path = self.ros_path + path = self.navcog_routes[self.route_index][0] path.header.stamp = CaBotRclpyUtil.now().to_msg() self.delegate.publish_path(path) CaBotRclpyUtil.info("NavGoal publish path") @@ -566,17 +799,21 @@ def enter(self): # bt_navigator will path only a pair of consecutive poses in the path to the plugin # so we use navigate_to_pose and planner will listen the published path # self.delegate.navigate_through_poses(self.ros_path.poses[1:], NavGoal.DEFAULT_BT_XML, self.done_callback) - self.delegate.navigate_to_pose(self.ros_path.poses[-1], NavGoal.DEFAULT_BT_XML, self.goal_handle_callback, self.done_callback) + self.delegate.navigate_to_pose(path.poses[-1], NavGoal.DEFAULT_BT_XML, self.goal_handle_callback, self.done_callback) def done_callback(self, future): if self.prevent_callback: self.prevent_callback = False return - CaBotRclpyUtil.info(F"NavGoal completed result={future.result()}") - status = future.result().status - self._is_completed = (status == GoalStatus.STATUS_SUCCEEDED) - self._is_canceled = (status != GoalStatus.STATUS_SUCCEEDED) + CaBotRclpyUtil.info(F"NavGoal completed result={future.result()}, {self.route_index}/{len(self.navcog_routes)}") + self.route_index += 1 + if self.route_index == len(self.navcog_routes): + status = future.result().status + self._is_completed = (status == GoalStatus.STATUS_SUCCEEDED) + self._is_canceled = (status != GoalStatus.STATUS_SUCCEEDED) + else: + self.enter() def update_goal(self, goal): CaBotRclpyUtil.info("Updated goal position") @@ -888,74 +1125,6 @@ def completed(self, pose, floor): return self.same_floor(floor) and self.distance_to(pose) > self.set_forward_distance -class NarrowGoal(NavGoal): - NARROW_BT_XML = "package://cabot_bt/behavior_trees/navigate_for_narrow.xml" - LITTLE_NARROW_BT_XML = "package://cabot_bt/behavior_trees/navigate_for_little_narrow.xml" - - def __init__(self, delegate, navcog_route, anchor, **kwargs): - # if one of the link is very narrow, it needs to announce - self._need_narrow_announce = False - for link_or_node in navcog_route: - CaBotRclpyUtil.info(F"{link_or_node}") - if not isinstance(link_or_node, geojson.RouteLink): - continue - self._need_narrow_announce = self._need_narrow_announce or link_or_node.need_narrow_announce - - super(NarrowGoal, self).__init__(delegate, navcog_route, anchor, **kwargs) - - def enter(self): - CaBotRclpyUtil.info("NarrowGoal enter") - # reset social distance setting if necessary - if self.delegate.initial_social_distance is not None and self.delegate.current_social_distance is not None \ - and (self.delegate.initial_social_distance.x != self.delegate.current_social_distance.x or - self.delegate.initial_social_distance.y != self.delegate.current_social_distance.y): - msg = self.delegate.initial_social_distance - self.delegate.set_social_distance_pub.publish(msg) - - CaBotRclpyUtil.info("NarrowGoal set social distance") - # publish navcog path - path = self.ros_path - path.header.stamp = CaBotRclpyUtil.now().to_msg() - self.delegate.publish_path(path) - CaBotRclpyUtil.info("NarrowGoal publish path") - - if self._need_narrow_announce: - self.delegate.please_follow_behind() - self.wait_for_announce() - else: - self.narrow_enter(NarrowGoal.LITTLE_NARROW_BT_XML) - - def done_callback(self, future): - CaBotRclpyUtil.info("NarrowGoal completed") - status = future.result().status - self._is_canceled = (status != GoalStatus.STATUS_SUCCEEDED) - if self._is_canceled: - return - if self._need_narrow_announce: - self.delegate.please_return_position() - self.wait_next_navi(future) - else: - self._is_completed = (status == GoalStatus.STATUS_SUCCEEDED) - - def narrow_enter(self, bt): - super(NavGoal, self).enter() - # wanted a path (not only a pose) in planner plugin, but it is not possible - # bt_navigator will path only a pair of consecutive poses in the path to the plugin - # so we use navigate_to_pose and planner will listen the published path - # basically the same as a NavGoal, use BT_XML that makes the footprint the same as an elevator to pass through narrow spaces - # self.delegate.navigate_through_poses(self.ros_path.poses[1:], NavGoal.DEFAULT_BT_XML, self.done_callback) - self.delegate.navigate_to_pose(self.ros_path.poses[-1], bt, self.goal_handle_callback, self.done_callback) - - @util.setInterval(5, times=1) - def wait_for_announce(self): - self.narrow_enter(NarrowGoal.NARROW_BT_XML) - - @util.setInterval(5, times=1) - def wait_next_navi(self, future): - status = future.result().status - self._is_completed = (status == GoalStatus.STATUS_SUCCEEDED) - - """ TODO class EscalatorInGoal(Goal): diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index db7e7a53..b51d4eac 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -28,8 +28,10 @@ # ROS import rclpy +from rcl_interfaces.srv import SetParameters from rclpy.node import Node from rclpy.action import ActionClient +from rclpy.parameter import Parameter, ParameterType from rclpy.qos import QoSProfile, QoSDurabilityPolicy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup import tf2_ros @@ -155,12 +157,12 @@ def lookup_transform(self, target, source, time=None): if key in self.transformMap: (transform, last_time) = self.transformMap[key] if now - last_time < self.min_interval: - self._logger.info(f"found old lookup_transform({target}, {source}, {(now - last_time).nanoseconds/1000000000:.2f}sec)") + self._logger.debug(f"found old lookup_transform({target}, {source}, {(now - last_time).nanoseconds/1000000000:.2f}sec)") return transform if __debug__: self.debug() - self._logger.info(f"lookup_transform({target}, {source})") + self._logger.debug(f"lookup_transform({target}, {source})") req = LookupTransform.Request() req.target_frame = target req.source_frame = source @@ -241,8 +243,7 @@ def current_ros_pose(self, frame=None): ros_pose.orientation.w = transformStamped.transform.rotation.w return ros_pose except RuntimeError: - self._logger.error(F"{self._node.get_clock().now()}") - self._logger.error(traceback.format_exc(), throttle_duration_sec=1.0) + self._logger.debug("cannot get current_ros_pose") raise RuntimeError("no transformation") def current_local_pose(self, frame=None): @@ -259,7 +260,7 @@ def current_local_pose(self, frame=None): current_pose = geoutil.Pose(x=translation.x, y=translation.y, r=euler[2]) return current_pose except RuntimeError: - self._logger.error(traceback.format_exc(), throttle_duration_sec=1.0) + self._logger.debug("cannot get current_local_pose") raise RuntimeError("no transformation") def current_local_odom_pose(self): @@ -273,7 +274,7 @@ def current_local_odom_pose(self): current_pose = geoutil.Pose(x=translation.x, y=translation.y, r=euler[2]) return current_pose except RuntimeError: - self._logger.error(traceback.format_exc(), throttle_duration_sec=1.0) + self._logger.debug("cannot get current_local_odom_pose") raise RuntimeError("no transformation") def current_global_pose(self): @@ -304,6 +305,8 @@ def __init__(self, node: Node, datautil_instance=None, anchor_file=None, wait_fo super(Navigation, self).__init__( node, datautil_instance=datautil_instance, anchor_file=anchor_file) + self.param_manager = NavigationParamManager(node) + self.info_pois = [] self.queue_wait_pois = [] self.speed_pois = [] @@ -541,7 +544,7 @@ def _set_destination(self, destination): self._goal_index = -1 # for dashboad - (gpath, _) = navgoal.create_ros_path(groute, self._anchor, self.global_map_name()) + (gpath, _, _) = navgoal.create_ros_path(groute, self._anchor, self.global_map_name()) msg = nav_msgs.msg.Path() msg.header = gpath.header msg.header.frame_id = "map" @@ -757,9 +760,6 @@ def _check_info_poi(self, current_pose): def _check_speed_limit(self, current_pose): # check speed limit - if not self.speed_pois: - return - limit = self._max_speed for poi in self.speed_pois: dist = poi.distance_to(current_pose) @@ -769,6 +769,10 @@ def _check_speed_limit(self, current_pose): else: limit = min(limit, self._max_speed) self._logger.debug(F"speed poi dist={dist:.2f}m, limit={limit:.2f}") + if self._current_goal: + if self._current_goal.speed_limit is not None and self._current_goal.speed_limit < limit: + limit = self._current_goal.speed_limit + msg = std_msgs.msg.Float32() msg.data = limit self.speed_limit_pub.publish(msg) @@ -1143,3 +1147,42 @@ def please_follow_behind(self): def please_return_position(self): self.delegate.please_return_position() + + def change_parameters(self, params, callback): + self.param_manager.change_parameters(params, callback) + + +class NavigationParamManager: + def __init__(self, node): + self.node = node + self.clients = {} + + def get_client(self, node_name): + if node_name not in self.clients: + self.clients[node_name] = self.node.create_client(SetParameters, f'{node_name}/set_parameters') + while not self.clients[node_name].wait_for_service(timeout_sec=1.0): + self.node.get_logger().info('Waiting for the target node to become available...') + return self.clients[node_name] + + def change_parameter(self, node_name, param_name, param_value, callback): + request = SetParameters.Request() + new_parameter = Parameter(param_name, value=param_value) + request.parameters.append(new_parameter.to_parameter_msg()) + future = self.get_client(node_name).call_async(request) + future.add_done_callback(callback) + + def change_parameters(self, params, callback): + self.count = 0 + + def sub_callback(future): + self.count += 1 + self.node.get_logger().info(f"sub_callback {self.count} {future.result()}") + if self.count == len(params): + callback() + for param in params: + param['callback'] = sub_callback + self.node.get_logger().info(f"call change_parameter {param['node_name']}: {param['param_name']} = {param['param_value']}") + try: + self.change_parameter(**param) + except: + self.node.get_logger().error(traceback.format_exc())