From 067e55b1941d55d0bbc21032a26e21375ac75fc8 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Thu, 8 Feb 2024 15:35:53 +0900 Subject: [PATCH 01/11] update pedestrian modules for implementing new test cases Signed-off-by: Masayuki Murata --- pedestrian_plugin/pedestrian/walk_across.py | 48 +++++++++++++++---- pedestrian_plugin/pedestrian/walk_straight.py | 44 ++++++++++++++--- 2 files changed, 76 insertions(+), 16 deletions(-) diff --git a/pedestrian_plugin/pedestrian/walk_across.py b/pedestrian_plugin/pedestrian/walk_across.py index 682e9cda..297c0aee 100644 --- a/pedestrian_plugin/pedestrian/walk_across.py +++ b/pedestrian_plugin/pedestrian/walk_across.py @@ -3,47 +3,75 @@ import math from pedestrian import state +stopped = False count=0 px=None py=None def onUpdate(**args): + global stopped global count, px, py + # parameters name = args['name'] - vel = args['velocity'] if 'velocity' in args else 1.0 - dt = args['dt'] + vel = args.get('velocity', 1.0) # [m/s] + collision_threshold = args.get('collision_threshold', 0.5) # [m] + decel_distance = args.get('decel_distance', 0.0) # [m] + pause_distance = args.get('pause_distance', 0.0) # [m] + stop_distance = args.get('stop_distance', 0.0) # [m] + + # variables + x = args['x'] + y = args['y'] yaw = args['yaw'] - threshold = args['collision_threshold'] if 'collision_threshold' in args else 0.5 + dt = args['dt'] + + # wait until robot pose is available + if 'robot' not in args: + return args rx = args['robot']['x'] ry = args['robot']['y'] - x = args['x'] - y = args['y'] + + # check collision dx = rx - x dy = ry - y dist = math.sqrt(dx * dx + dy * dy) - if dist < threshold: + if dist < collision_threshold: ros.info(f"collision {name} {dist}") ros.collision(name, dist) - dx = 0 + # apply decel & pause_distance (temporary slow down) + if dist < pause_distance: + vel = 0.0 + elif dist < decel_distance: + vel = (dist - pause_distance) / (decel_distance - pause_distance) * vel + + # apply stop_distance (permanent stop) + if dist <= stop_distance: + stopped = True + if stopped: + vel = 0.0 + + # wait until the robot starts to move if px is not None and py is not None: dx = rx - px dy = ry - py dist = math.sqrt(dx*dx+dy*dy) if dist > 0.01 or count > 0: - count = 100 + count = 10 else: vel = 0 - count-=1 + count -= 1 px = rx py = ry + # update actor variables args['x'] += vel * dt * math.cos(yaw) args['y'] += vel * dt * math.sin(yaw) args['yaw'] = yaw - name = args['name'] + + # save state state.state[name] = args # ros.info(f"{args} {px} {py} {dx}") diff --git a/pedestrian_plugin/pedestrian/walk_straight.py b/pedestrian_plugin/pedestrian/walk_straight.py index 44432273..825ceaeb 100644 --- a/pedestrian_plugin/pedestrian/walk_straight.py +++ b/pedestrian_plugin/pedestrian/walk_straight.py @@ -3,29 +3,61 @@ import math from pedestrian import state +stopped = False + def onUpdate(**args): - vel = args['velocity'] if 'velocity' in args else 1.0 - dt = args['dt'] + global stopped + + # parameters + name = args['name'] + vel = args.get('velocity', 1.0) # [m/s] + threshold = args.get('collision_threshold', 0.5) # [m] + decel_distance = args.get('decel_distance', 0.0) # [m] + pause_distance = args.get('pause_distance', 0.0) # [m] + stop_distance = args.get('stop_distance', 0.0) # [m] + + # plugin variables + x = args['x'] + y = args['y'] yaw = args['yaw'] + dt = args['dt'] + # wait until robot pose is available if 'robot' not in args: return args + # plugin variables rx = args['robot']['x'] ry = args['robot']['y'] - x = args['x'] - y = args['y'] dx = rx - x dy = ry - y dist = math.sqrt(dx*dx+dy*dy) - vel = min(max(0, dist-1.5), vel) + # check collision + if dist < threshold: + ros.info(f"collision {name} {dist}") + ros.collision(name, dist) + + # apply decel & pause_distance (temporary slow down) + if dist < pause_distance: + vel = 0.0 + elif dist < decel_distance: + vel = (dist - pause_distance) / (decel_distance - pause_distance) * vel + + # apply stop_distance (permanent stop) + if dist <= stop_distance: + stopped = True + if stopped: + vel = 0.0 + + # update actor variables args['x'] += vel * dt * math.cos(yaw) args['y'] += vel * dt * math.sin(yaw) args['yaw'] = yaw - name = args['name'] + + # save state state.state[name] = args #ros.info(f"{args}") From 6e941eb879ceee2118b433d141a595d79e2c3d6b Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Fri, 9 Feb 2024 02:36:06 +0900 Subject: [PATCH 02/11] update walk_sfm module Signed-off-by: Masayuki Murata --- pedestrian_plugin/pedestrian/walk_sfm.py | 48 +++++++++++++++--------- 1 file changed, 30 insertions(+), 18 deletions(-) diff --git a/pedestrian_plugin/pedestrian/walk_sfm.py b/pedestrian_plugin/pedestrian/walk_sfm.py index 8ec5ec48..5ad9aa76 100644 --- a/pedestrian_plugin/pedestrian/walk_sfm.py +++ b/pedestrian_plugin/pedestrian/walk_sfm.py @@ -15,8 +15,9 @@ pRobot = None # previous robot state # debug -count=0 -initialized=False +count = 0 +initialized = False + def onUpdate(**args): global count, pRobot, initialized @@ -24,10 +25,22 @@ def onUpdate(**args): state.state.clear() initialized = True - dt = args['dt'] + # parameter + collision_threshold = args['collision_threshold'] if 'collision_threshold' in args else 0.5 + n_actors = args.get("n_actors", 10) + goal_x = args.get('goal_x', None) + goal_y = args.get('goal_y', None) + min_x = args.get('min_x', -np.inf) + max_x = args.get('max_x', np.inf) + min_y = args.get('min_y', -np.inf) + max_y = args.get('max_y', np.inf) + + args['goal_x'] = goal_x + args['goal_y'] = goal_y + name = args['name'] + dt = args['dt'] robot = args['robot'] if 'robot' in args else None - collision_threshold = args['collision_threshold'] if 'collision_threshold' in args else 0.5 if robot: rx = robot['x'] @@ -42,7 +55,7 @@ def onUpdate(**args): # here will be executed for all actors # but wait until all actors state is available - if len(state.state.keys()) == 10: + if len(state.state.keys()) == n_actors: # build state array for simulation temp = [] index = 0 @@ -54,11 +67,11 @@ def onUpdate(**args): vel = args['velocity'] vx = vel*math.cos(yaw) vy = vel*math.sin(yaw) - gx = x + math.cos(yaw) - gy = y + math.sin(yaw) + gx = x + math.cos(yaw) if st['goal_x'] is None else st['goal_x'] + gy = y + math.sin(yaw) if st['goal_y'] is None else st['goal_y'] temp.append([x, y, vx, vy, gx, gy]) indicies[key] = index - index+=1 + index += 1 # add robot state if pRobot: rx = robot['x'] @@ -72,25 +85,25 @@ def onUpdate(**args): grx = robot['x'] + math.cos(yaw) gry = robot['y'] + math.sin(yaw) temp.append([rx, ry, rvx, rvy, grx, gry]) - + # here needs to be hacked a bit sim.peds = pysocialforce.scene.PedState(np.array(temp), None, sim.scene_config) sim.forces = sim.make_forces(sim.config) sim.step(1) - + # debug outpu if count == 0: ros.info(f"{robot}") ros.info(f"{sim.config}") ros.info(f"{sim.peds.state}") - count=1 + count = 1 # clear the state to ensure the simulation is executed only once in a cycle state.state.clear() pRobot = robot if name in indicies: - index=indicies[name] + index = indicies[name] st = sim.peds.state[index] x = st[0] y = st[1] @@ -99,16 +112,15 @@ def onUpdate(**args): v = math.sqrt(vx*vx+vy*vy) yaw = math.atan2(vy, vx) - # bound in (-5, -5) (5, 5) region - bound=5 - if bound < x and abs(yaw) < math.pi/2: + # bound in (min_x, min_y) x (max_x, max_y) region + if max_x < x and abs(yaw) < math.pi/2: yaw = yaw+math.pi - if x < -bound and math.pi/2 < abs(yaw) : + if x < min_x and math.pi/2 < abs(yaw): yaw = yaw-math.pi - if bound < y and 0 < yaw: + if max_y < y and 0 < yaw: yaw = -yaw - if y < -bound and yaw < 0: + if y < min_y and yaw < 0: yaw = -yaw if math.pi < yaw: From 012dbf018caf7db63182c27f6c07e1ea112e2a80 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Tue, 20 Feb 2024 17:57:37 +0900 Subject: [PATCH 03/11] add an interface to publish metric to the pedestrian plugin Signed-off-by: Masayuki Murata --- .../pedestrian_plugin_manager.hpp | 3 +++ pedestrian_plugin/src/pedestrian_plugin.cpp | 1 + .../src/pedestrian_plugin_manager.cpp | 21 +++++++++++++++++ pedestrian_plugin_msgs/CMakeLists.txt | 1 + pedestrian_plugin_msgs/msg/Metric.msg | 23 +++++++++++++++++++ 5 files changed, 49 insertions(+) create mode 100644 pedestrian_plugin_msgs/msg/Metric.msg diff --git a/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp b/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp index 18723eaa..eafd5239 100644 --- a/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp +++ b/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp @@ -30,6 +30,7 @@ #include #include #include +#include #include PyObject* PyInit_ros(void); @@ -94,6 +95,7 @@ class PedestrianPluginManager { void updatePersonMessage(std::string name, people_msgs::msg::Person person); rclcpp::Logger get_logger(); void process_collision(std::string actor_name, double distance); + void process_metric(std::string name, double value); std::recursive_mutex mtx; @@ -109,6 +111,7 @@ class PedestrianPluginManager { std::map peopleReadyMap_; rclcpp::Publisher::SharedPtr people_pub_; rclcpp::Publisher::SharedPtr collision_pub_; + rclcpp::Publisher::SharedPtr metric_pub_; rclcpp::Service::SharedPtr service_; }; diff --git a/pedestrian_plugin/src/pedestrian_plugin.cpp b/pedestrian_plugin/src/pedestrian_plugin.cpp index 198510bf..16213e5a 100644 --- a/pedestrian_plugin/src/pedestrian_plugin.cpp +++ b/pedestrian_plugin/src/pedestrian_plugin.cpp @@ -207,6 +207,7 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) PyDict_SetItemString(pDict, "robot", pRobotPose); Py_DECREF(pRobotPose); } + PythonUtils::setDictItemAsFloat(pDict, "time", _info.simTime.Float()); PythonUtils::setDictItemAsFloat(pDict, "dt", dt); PythonUtils::setDictItemAsFloat(pDict, "x", this->x); PythonUtils::setDictItemAsFloat(pDict, "y", this->y); diff --git a/pedestrian_plugin/src/pedestrian_plugin_manager.cpp b/pedestrian_plugin/src/pedestrian_plugin_manager.cpp index 0bd55298..7c252f1b 100644 --- a/pedestrian_plugin/src/pedestrian_plugin_manager.cpp +++ b/pedestrian_plugin/src/pedestrian_plugin_manager.cpp @@ -47,8 +47,21 @@ PyObject* ros_collision(PyObject* self, PyObject* args) { Py_RETURN_NONE; } +PyObject* ros_metric(PyObject* self, PyObject* args) { + PyObject* unicode_obj; + double value; + if (!PyArg_ParseTuple(args, "Ud", &unicode_obj, &value)) { + RCLCPP_INFO(PedestrianPluginManager::getInstance().get_logger(), "error in parsing tuple"); + return NULL; + } + std::string name = PythonUtils::PyUnicodeObject_ToStdString(unicode_obj); + PedestrianPluginManager::getInstance().process_metric(name, value); + Py_RETURN_NONE; +} + PyMethodDef RosMethods[] = {{"info", ros_info, METH_VARARGS, "call RCLCPP_INFO"}, {"collision", ros_collision, METH_VARARGS, "publish collision message"}, + {"metric", ros_metric, METH_VARARGS, "publish metric message"}, {NULL, NULL, 0, NULL}}; PyModuleDef RosModule = {PyModuleDef_HEAD_INIT, "ros", NULL, -1, RosMethods, NULL, NULL, NULL, NULL}; @@ -106,6 +119,7 @@ ParamValue PedestrianPluginParams::getParam(std::string name) { PedestrianPluginManager::PedestrianPluginManager() : node_(gazebo_ros::Node::Get()) { people_pub_ = node_->create_publisher("/people", 10); collision_pub_ = node_->create_publisher("/collision", 10); + metric_pub_ = node_->create_publisher("/metric", 10); service_ = node_->create_service( "/pedestrian_plugin_update", std::bind(&PedestrianPluginManager::handle_plugin_update, this, std::placeholders::_1, std::placeholders::_2)); @@ -165,6 +179,13 @@ void PedestrianPluginManager::process_collision(std::string actor_name, double d collision_pub_->publish(msg); } +void PedestrianPluginManager::process_metric(std::string name, double value) { + pedestrian_plugin_msgs::msg::Metric msg; + msg.name = name; + msg.value = value; + metric_pub_->publish(msg); +} + // Private methods void PedestrianPluginManager::handle_plugin_update( diff --git a/pedestrian_plugin_msgs/CMakeLists.txt b/pedestrian_plugin_msgs/CMakeLists.txt index 139b527b..3914c402 100644 --- a/pedestrian_plugin_msgs/CMakeLists.txt +++ b/pedestrian_plugin_msgs/CMakeLists.txt @@ -16,6 +16,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} msg/Collision.msg + msg/Metric.msg msg/Plugin.msg msg/PluginParam.msg srv/PluginUpdate.srv diff --git a/pedestrian_plugin_msgs/msg/Metric.msg b/pedestrian_plugin_msgs/msg/Metric.msg new file mode 100644 index 00000000..68a91a34 --- /dev/null +++ b/pedestrian_plugin_msgs/msg/Metric.msg @@ -0,0 +1,23 @@ +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +std_msgs/Header header +string name +float64 value \ No newline at end of file From 9c06c4dfc072cdbdbb2fac72d2f766071b1ecd50 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 21 Feb 2024 01:11:00 +0900 Subject: [PATCH 04/11] add robot and human publishers to the pedestrian plugin Signed-off-by: Masayuki Murata --- .../pedestrian_plugin_manager.hpp | 11 +++++ pedestrian_plugin/pedestrian/walk_across.py | 2 + pedestrian_plugin/pedestrian/walk_sfm.py | 2 + pedestrian_plugin/pedestrian/walk_straight.py | 2 + pedestrian_plugin/src/pedestrian_plugin.cpp | 48 +++++++++++++++++++ .../src/pedestrian_plugin_manager.cpp | 34 +++++++++++++ pedestrian_plugin_msgs/CMakeLists.txt | 2 + pedestrian_plugin_msgs/msg/Agent.msg | 38 +++++++++++++++ pedestrian_plugin_msgs/msg/Agents.msg | 22 +++++++++ 9 files changed, 161 insertions(+) create mode 100644 pedestrian_plugin_msgs/msg/Agent.msg create mode 100644 pedestrian_plugin_msgs/msg/Agents.msg diff --git a/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp b/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp index eafd5239..d282a1fd 100644 --- a/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp +++ b/pedestrian_plugin/include/pedestrian_plugin/pedestrian_plugin_manager.hpp @@ -26,11 +26,14 @@ #include #include +#include #include #include #include #include #include +#include +#include #include PyObject* PyInit_ros(void); @@ -93,6 +96,9 @@ class PedestrianPluginManager { void publishPeopleIfReady(); void updateRobotPose(geometry_msgs::msg::Pose robot_pose); void updatePersonMessage(std::string name, people_msgs::msg::Person person); + void updateStamp(builtin_interfaces::msg::Time stamp); + void updateRobotAgent(pedestrian_plugin_msgs::msg::Agent robotAgent); + void updateHumanAgent(std::string name, pedestrian_plugin_msgs::msg::Agent humanAgent); rclcpp::Logger get_logger(); void process_collision(std::string actor_name, double distance); void process_metric(std::string name, double value); @@ -109,9 +115,14 @@ class PedestrianPluginManager { std::map pluginMap_; std::map peopleMap_; std::map peopleReadyMap_; + builtin_interfaces::msg::Time::SharedPtr stamp_; + pedestrian_plugin_msgs::msg::Agent::SharedPtr robotAgent_; + std::map humanAgentsMap_; rclcpp::Publisher::SharedPtr people_pub_; rclcpp::Publisher::SharedPtr collision_pub_; rclcpp::Publisher::SharedPtr metric_pub_; + rclcpp::Publisher::SharedPtr robot_pub_; + rclcpp::Publisher::SharedPtr human_pub_; rclcpp::Service::SharedPtr service_; }; diff --git a/pedestrian_plugin/pedestrian/walk_across.py b/pedestrian_plugin/pedestrian/walk_across.py index 297c0aee..8701ec2e 100644 --- a/pedestrian_plugin/pedestrian/walk_across.py +++ b/pedestrian_plugin/pedestrian/walk_across.py @@ -19,6 +19,8 @@ def onUpdate(**args): decel_distance = args.get('decel_distance', 0.0) # [m] pause_distance = args.get('pause_distance', 0.0) # [m] stop_distance = args.get('stop_distance', 0.0) # [m] + radius = args.get('radius', 0.4) # [m] + args['radius'] = radius # variables x = args['x'] diff --git a/pedestrian_plugin/pedestrian/walk_sfm.py b/pedestrian_plugin/pedestrian/walk_sfm.py index 5ad9aa76..87c0a04b 100644 --- a/pedestrian_plugin/pedestrian/walk_sfm.py +++ b/pedestrian_plugin/pedestrian/walk_sfm.py @@ -34,9 +34,11 @@ def onUpdate(**args): max_x = args.get('max_x', np.inf) min_y = args.get('min_y', -np.inf) max_y = args.get('max_y', np.inf) + radius = args.get('radius', 0.4) # [m] args['goal_x'] = goal_x args['goal_y'] = goal_y + args['radius'] = radius name = args['name'] dt = args['dt'] diff --git a/pedestrian_plugin/pedestrian/walk_straight.py b/pedestrian_plugin/pedestrian/walk_straight.py index 825ceaeb..81d4cc3a 100644 --- a/pedestrian_plugin/pedestrian/walk_straight.py +++ b/pedestrian_plugin/pedestrian/walk_straight.py @@ -15,6 +15,8 @@ def onUpdate(**args): decel_distance = args.get('decel_distance', 0.0) # [m] pause_distance = args.get('pause_distance', 0.0) # [m] stop_distance = args.get('stop_distance', 0.0) # [m] + radius = args.get('radius', 0.4) # [m] + args['radius'] = radius # plugin variables x = args['x'] diff --git a/pedestrian_plugin/src/pedestrian_plugin.cpp b/pedestrian_plugin/src/pedestrian_plugin.cpp index 16213e5a..6af0e51e 100644 --- a/pedestrian_plugin/src/pedestrian_plugin.cpp +++ b/pedestrian_plugin/src/pedestrian_plugin.cpp @@ -174,6 +174,8 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) if (dt < 0.05) { // 20hz return; } + auto stamp = gazebo_ros::Convert(_info.simTime); + manager.updateStamp(stamp); manager.publishPeopleIfReady(); this->lastUpdate = _info.simTime; @@ -189,12 +191,20 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) ignition::math::Vector3d rPos = robotModel->WorldPose().Pos(); ignition::math::Quaterniond rRot = robotModel->WorldPose().Rot(); ignition::math::Vector3d rRpy = rRot.Euler(); + ignition::math::Vector3d linearVel = robotModel->WorldLinearVel(); + auto vel_x = linearVel.X(); + auto vel_y = linearVel.Y(); + auto vel_linear = linearVel.Length(); + ignition::math::Vector3d angularVel = robotModel->WorldAngularVel(); + auto vel_theta = angularVel.Z(); PythonUtils::setDictItemAsFloat(pRobotPose, "x", rPos.X()); PythonUtils::setDictItemAsFloat(pRobotPose, "y", rPos.Y()); PythonUtils::setDictItemAsFloat(pRobotPose, "z", rPos.Z()); PythonUtils::setDictItemAsFloat(pRobotPose, "roll", rRpy.X()); PythonUtils::setDictItemAsFloat(pRobotPose, "pitch", rRpy.X()); PythonUtils::setDictItemAsFloat(pRobotPose, "yaw", rRpy.X()); + PythonUtils::setDictItemAsFloat(pRobotPose, "vel_linear", vel_linear); + PythonUtils::setDictItemAsFloat(pRobotPose, "vel_theta", vel_theta); geometry_msgs::msg::Pose robot_pose; robot_pose.position.x = rPos.X(); robot_pose.position.y = rPos.Y(); @@ -204,6 +214,22 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) robot_pose.orientation.z = rRot.Z(); robot_pose.orientation.w = rRot.W(); manager.updateRobotPose(robot_pose); + + // update robot agent + pedestrian_plugin_msgs::msg::Agent robotAgent; + robotAgent.type = pedestrian_plugin_msgs::msg::Agent::ROBOT; + robotAgent.behavior_state = pedestrian_plugin_msgs::msg::Agent::ACTIVE; + robotAgent.name = std::string("robot"); + robotAgent.position = robot_pose; + robotAgent.yaw = rRpy.X(); + robotAgent.velocity.linear.x = vel_x; + robotAgent.velocity.linear.y = vel_y; + robotAgent.velocity.angular.z = vel_theta; + robotAgent.linear_vel = vel_linear; + robotAgent.angular_vel = vel_theta; + robotAgent.radius = 0.45; // default robot raduis + manager.updateRobotAgent(robotAgent); + PyDict_SetItemString(pDict, "robot", pRobotPose); Py_DECREF(pRobotPose); } @@ -236,6 +262,7 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) auto newRoll = PythonUtils::getDictItemAsDouble(pRet, "roll", 0.0); auto newPitch = PythonUtils::getDictItemAsDouble(pRet, "pitch", 0.0); auto newYaw = PythonUtils::getDictItemAsDouble(pRet, "yaw", 0.0); + auto radius = PythonUtils::getDictItemAsDouble(pRet, "radius", 0.4); auto dx = newX - this->x; auto dy = newY - this->y; auto dz = newZ - this->z; @@ -275,6 +302,27 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) } manager.updatePersonMessage(this->actor->GetName(), person); + // update human agent + double vel_linear = dd / dt; + pedestrian_plugin_msgs::msg::Agent humanAgent; + humanAgent.type = pedestrian_plugin_msgs::msg::Agent::PERSON; + if (this->module_name == "pedestrian.pool"){ + humanAgent.behavior_state = pedestrian_plugin_msgs::msg::Agent::INACTIVE; + }else{ + humanAgent.behavior_state = pedestrian_plugin_msgs::msg::Agent::ACTIVE; + } + humanAgent.name = person.name; + humanAgent.position.position = person.position; + humanAgent.yaw = newYaw; + humanAgent.velocity.linear.x = person.velocity.x; + humanAgent.velocity.linear.y = person.velocity.y; + humanAgent.velocity.linear.z = person.velocity.z; + humanAgent.linear_vel = vel_linear; + // humanAgent.velocity.angular.z // undefined + // humanAgent.angular_vel // undefined + humanAgent.radius = radius; + manager.updateHumanAgent(person.name, humanAgent); + Py_DECREF(pRet); } else { PyErr_Print(); diff --git a/pedestrian_plugin/src/pedestrian_plugin_manager.cpp b/pedestrian_plugin/src/pedestrian_plugin_manager.cpp index 7c252f1b..a74cf933 100644 --- a/pedestrian_plugin/src/pedestrian_plugin_manager.cpp +++ b/pedestrian_plugin/src/pedestrian_plugin_manager.cpp @@ -120,6 +120,8 @@ PedestrianPluginManager::PedestrianPluginManager() : node_(gazebo_ros::Node::Get people_pub_ = node_->create_publisher("/people", 10); collision_pub_ = node_->create_publisher("/collision", 10); metric_pub_ = node_->create_publisher("/metric", 10); + robot_pub_ = node_->create_publisher("/robot_states", 10); + human_pub_ = node_->create_publisher("/human_states", 10); service_ = node_->create_service( "/pedestrian_plugin_update", std::bind(&PedestrianPluginManager::handle_plugin_update, this, std::placeholders::_1, std::placeholders::_2)); @@ -140,8 +142,22 @@ void PedestrianPluginManager::publishPeopleIfReady() { for (auto it : peopleMap_) { msg.people.push_back(it.second); } + msg.header.stamp = *stamp_; msg.header.frame_id = "map_global"; people_pub_->publish(msg); + + // publish robot and human messages + robotAgent_->header.stamp = *stamp_; + robotAgent_->header.frame_id = "map_global"; + robot_pub_->publish(*robotAgent_); + pedestrian_plugin_msgs::msg::Agents humanAgentsMsg; + for (auto it: humanAgentsMap_){ + humanAgentsMsg.agents.push_back(it.second); + } + humanAgentsMsg.header.stamp = *stamp_; + humanAgentsMsg.header.frame_id = "map_global"; + human_pub_->publish(humanAgentsMsg); + peopleReadyMap_.clear(); } } @@ -159,6 +175,24 @@ void PedestrianPluginManager::updatePersonMessage(std::string name, people_msgs: peopleReadyMap_.insert({name, true}); } +void PedestrianPluginManager::updateStamp(builtin_interfaces::msg::Time stamp) { + if (stamp_ == nullptr) { + stamp_ = std::make_shared(); + } + *stamp_ = stamp; +} + +void PedestrianPluginManager::updateRobotAgent(pedestrian_plugin_msgs::msg::Agent robotAgent) { + if (robotAgent_ == nullptr) { + robotAgent_ = std::make_shared(); + } + *robotAgent_ = robotAgent; +} + +void PedestrianPluginManager::updateHumanAgent(std::string name, pedestrian_plugin_msgs::msg::Agent humanAgent) { + humanAgentsMap_.insert_or_assign(name, humanAgent); +} + rclcpp::Logger PedestrianPluginManager::get_logger() { return node_->get_logger(); } void PedestrianPluginManager::process_collision(std::string actor_name, double distance) { diff --git a/pedestrian_plugin_msgs/CMakeLists.txt b/pedestrian_plugin_msgs/CMakeLists.txt index 3914c402..acf651e1 100644 --- a/pedestrian_plugin_msgs/CMakeLists.txt +++ b/pedestrian_plugin_msgs/CMakeLists.txt @@ -15,6 +15,8 @@ if(BUILD_TESTING) endif() rosidl_generate_interfaces(${PROJECT_NAME} + msg/Agent.msg + msg/Agents.msg msg/Collision.msg msg/Metric.msg msg/Plugin.msg diff --git a/pedestrian_plugin_msgs/msg/Agent.msg b/pedestrian_plugin_msgs/msg/Agent.msg new file mode 100644 index 00000000..461a73be --- /dev/null +++ b/pedestrian_plugin_msgs/msg/Agent.msg @@ -0,0 +1,38 @@ +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +# types +uint8 PERSON=1 +uint8 ROBOT=2 + +# behavior states +uint8 INACTIVE=0 +uint8 ACTIVE=1 + +std_msgs/Header header +uint8 type +uint8 behavior_state +string name +geometry_msgs/Pose position +float32 yaw +geometry_msgs/Twist velocity +float32 radius +float32 linear_vel +float32 angular_vel \ No newline at end of file diff --git a/pedestrian_plugin_msgs/msg/Agents.msg b/pedestrian_plugin_msgs/msg/Agents.msg new file mode 100644 index 00000000..b8cb0a80 --- /dev/null +++ b/pedestrian_plugin_msgs/msg/Agents.msg @@ -0,0 +1,22 @@ +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +std_msgs/Header header +pedestrian_plugin_msgs/Agent[] agents From 033b48c04a4b48fc36124945112c7a048da86684 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 21 Feb 2024 13:47:27 +0900 Subject: [PATCH 05/11] bugfix of roll, pitch, yaw in the pedestrian plugin Signed-off-by: Masayuki Murata --- pedestrian_plugin/src/pedestrian_plugin.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/pedestrian_plugin/src/pedestrian_plugin.cpp b/pedestrian_plugin/src/pedestrian_plugin.cpp index 6af0e51e..6887afa3 100644 --- a/pedestrian_plugin/src/pedestrian_plugin.cpp +++ b/pedestrian_plugin/src/pedestrian_plugin.cpp @@ -201,8 +201,8 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) PythonUtils::setDictItemAsFloat(pRobotPose, "y", rPos.Y()); PythonUtils::setDictItemAsFloat(pRobotPose, "z", rPos.Z()); PythonUtils::setDictItemAsFloat(pRobotPose, "roll", rRpy.X()); - PythonUtils::setDictItemAsFloat(pRobotPose, "pitch", rRpy.X()); - PythonUtils::setDictItemAsFloat(pRobotPose, "yaw", rRpy.X()); + PythonUtils::setDictItemAsFloat(pRobotPose, "pitch", rRpy.Y()); + PythonUtils::setDictItemAsFloat(pRobotPose, "yaw", rRpy.Z()); PythonUtils::setDictItemAsFloat(pRobotPose, "vel_linear", vel_linear); PythonUtils::setDictItemAsFloat(pRobotPose, "vel_theta", vel_theta); geometry_msgs::msg::Pose robot_pose; @@ -221,7 +221,7 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) robotAgent.behavior_state = pedestrian_plugin_msgs::msg::Agent::ACTIVE; robotAgent.name = std::string("robot"); robotAgent.position = robot_pose; - robotAgent.yaw = rRpy.X(); + robotAgent.yaw = rRpy.Z(); robotAgent.velocity.linear.x = vel_x; robotAgent.velocity.linear.y = vel_y; robotAgent.velocity.angular.z = vel_theta; From 3b40ceb1eefbbe15d19c1e1ca49d57db096d034b Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 21 Feb 2024 15:14:14 +0900 Subject: [PATCH 06/11] adopt hunav_metrics as initial implementation Signed-off-by: Masayuki Murata --- cabot_navigation2/test/evaluation_metrics.py | 682 +++++++++++++++++++ 1 file changed, 682 insertions(+) create mode 100644 cabot_navigation2/test/evaluation_metrics.py diff --git a/cabot_navigation2/test/evaluation_metrics.py b/cabot_navigation2/test/evaluation_metrics.py new file mode 100644 index 00000000..e2962bf1 --- /dev/null +++ b/cabot_navigation2/test/evaluation_metrics.py @@ -0,0 +1,682 @@ +#!/usr/bin/env python3 + +############################################################################### +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +############################################################################### + +# metrics implementation is based on Hunav Evaluator (MIT license) +# https://github.com/robotics-upo/hunav_sim/tree/humble/hunav_evaluator + +from importlib.resources import path +from tokenize import group +import numpy as np +import math +import sys +import rclpy +from hunav_msgs.msg import Agents +from hunav_msgs.msg import Agent +from geometry_msgs.msg import Pose +from hunav_evaluator.sfm import SFM + +# Teaching Robot Navigation Behaviors to Optimal RRT Planners +# Noé Pérez-Higueras, Fernando Caballero & Luis Merino + +def euclidean_distance(pose, pose1): + return math.sqrt((pose.position.x - pose1.position.x)**2 + (pose.position.y - pose1.position.y)**2) + + +def get_group_center(agents_i, group_id, distance): + + group = [] + for agent in agents_i: + if agent.group_id == group_id: + pose = Pose() + pose.position.x = agent.position.position.x + (distance * math.cos(agent.yaw)) + pose.position.y = agent.position.position.y + (distance * math.sin(agent.yaw)) + group.append(pose) + + interaction_center = Pose() + for p in group: + interaction_center.position.x += p.position.x + interaction_center.position.y += p.position.y + + interaction_center.position.x = float(interaction_center.position.x/len(group)) + interaction_center.position.y = float(interaction_center.position.y/len(group)) + return interaction_center + + + +def indicator_function(norm, k): + if k == 'intimate': + if norm < 0.45: + return 1 + else: + return 0 + elif k == 'personal': + if norm >= 0.45 and norm < 1.2: + return 1 + else: + return 0 + elif k == 'social': + if norm >= 1.2 and norm < 3.6: + return 1 + else: + return 0 + elif k == 'public': + if norm >= 3.6: + return 1 + else: + return 0 + else: + return 0 + + +def get_time_stamps(agents, robot): + time_list = [] + t0 = rclpy.time.Time.from_msg(agents[0].header.stamp) + for a in agents: + t = rclpy.time.Time.from_msg(a.header.stamp) + dur = (t - t0).to_msg() + s = float(dur.sec + dur.nanosec/1e9) + time_list.append(s) + return time_list + +def total_time(agents, robot): + t2 = rclpy.time.Time.from_msg(agents[len(agents)-1].header.stamp) + t1 = rclpy.time.Time.from_msg(agents[0].header.stamp) + dur = (t2 - t1).to_msg() + secs = float(dur.sec + dur.nanosec/1e9) + print('\nTime_to_reach_goal computed: %.2f secs' % secs) + return [secs] + +def robot_path_length(agents, robot): + path_length = 0.0 + for i in range(len(robot)-1): + path_length += euclidean_distance(robot[i+1].position, robot[i].position) + print('Path_length computed: %.2f m' % path_length) + return [path_length] + +def cumulative_heading_changes(agents, robot): + chc_list = [0.0] + chc = 0 + for i in range(len(robot) - 1): + norm = normalize_angle(robot[i].yaw - robot[i+1].yaw) + if norm < 0.0: + norm *= -1 + chc += norm + chc_list.append(norm) + + print('Cumulative_heading_changes: %.2f rads' % chc) + return [chc, chc_list] + +def normalize_angle(ang): + while (ang <= -math.pi): + ang += 2 * math.pi + while (ang > math.pi): + ang -= 2 * math.pi + return ang + +def avg_closest_person(agents, robot): + min_dist_list = [] + avg_dist = 0 + for i in range(len(robot)): + min_dist = 10000 + for agent in agents[i].agents: + d = euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius + if(d < min_dist): + min_dist = d + if min_dist < 0.0: + min_dist = 0.0 + if(len(agents[i].agents)>0): + avg_dist += min_dist + min_dist_list.append(min_dist) + + avg_dist = avg_dist/len(robot) + print('Average_closest_person: %.2f m' % avg_dist) + return [avg_dist, min_dist_list] + + +def minimum_distance_to_people(agents, robot): + min_distance = list() + + for i in range(len(robot)): + for agent in agents[i].agents: + d = euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius + if d<0.0: + d = 0.0 + min_distance.append(d) + + min_dist = min(min_distance) + + print('Minimum_distance_to_people: %.2f m' % min_dist) + + return [min_dist] + +def maximum_distance_to_people(agents, robot): + max_distance = list() + + for i in range(len(robot)): + for agent in agents[i].agents: + max_distance.append(euclidean_distance(robot[i].position, agent.position) - robot[i].radius) + + max_dist = max(max_distance) + + print('Maximum_distance_to_people: %.2f m' % max_dist) + + return [max_dist] + + + +def space_intrusions(agents, robot, k): + space_intrusions = 0 + space_intrusions_list = [0] * len(robot) + + for i in range(len(robot)): + min_dist = 10000 + for agent in agents[i].agents: + d = euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius + if d < min_dist: + min_dist = d + if min_dist < 0.0: + min_dist = 0.0 + indicator = indicator_function(min_dist, k) + if indicator == 1: + space_intrusions += 1 + space_intrusions_list[i]=1 + + space_intrusions = space_intrusions / len(robot) + percentage = space_intrusions * 100.0 + + return percentage, space_intrusions_list + + +def intimate_space_intrusions(agents, robot): + percentage, slist = space_intrusions(agents, robot, 'intimate') + print('Intimate_space_intrusions: %.2f %% of the total time' % percentage) + return [percentage, slist] + +def personal_space_intrusions(agents, robot): + percentage, slist = space_intrusions(agents, robot, 'personal') + print('Personal_space_intrusions: %.2f %% of the total time' % percentage) + return [percentage, slist] + +def social_space_intrusions(agents, robot): + percentage, slist = space_intrusions(agents, robot, 'social') + print('Social_space_intrusions: %.2f %% of the total time' % percentage) + return [percentage, slist] + + + +def detect_groups(agents): + group_ids = [] + for a in agents[0].agents: + if(a.group_id != -1 and ((a.group_id in group_ids) == False)): + group_ids.append(a.group_id) + + return group_ids + + +def group_space_intrusions(agents, robot, k): + group_ids = detect_groups(agents) + if(len(group_ids)==0): + return [0.0] + + d=1.5 + space_intrusions = 0 + group_list = [0] * len(robot) + for i in range(len(robot)): + min_dist = 10000 + for id in group_ids: + group_center = get_group_center(agents[i].agents, id, d) + dist = euclidean_distance(robot[i].position, group_center.position) - robot[i].radius + if dist < min_dist: + min_dist = dist + indicator = indicator_function(min_dist, k) + if indicator == 1: + space_intrusions += 1 + group_list[i] = 1 + + space_intrusions = space_intrusions / len(robot) + percentage = space_intrusions * 100.0 + + return [percentage, group_list] + + +def group_intimate_space_intrusions(agents, robot): + r = group_space_intrusions(agents, robot, 'intimate') + print('Group_intimate_space_intrusions: %.2f %% of the total time' % r[0]) + return r + +def group_personal_space_intrusions(agents, robot): + r = group_space_intrusions(agents, robot, 'personal') + print('Group_personal_space_intrusions: %.2f %% of the total time' % r[0]) + return r + +def group_social_space_intrusions(agents, robot): + r = group_space_intrusions(agents, robot, 'social') + print('Group_social_space_intrusions: %.2f %% of the total time' % r[0]) + return r + + + + + +# SEAN 2.0: Formalizing and Generating Social Situations for Robot Navigation +# Nathan Tsoi, Alec Xiang, Peter Yu, Samuel S. Sohn, Greg Schwartz, Subashri Ramesh, Mohamed Hussein, Anjali W. Gupta, Mubbasir Kapadia, and Marynel Vázquez + +# The metrics Robot on Person Personal Distance Violation, Person on Robot Personal Distance Violation, Intimate Distance Violation and +# Person on Robot Intimate Distance Violation have already been implemented in the Personal_space_intrusions function. +# Instead of returning the number of times, it returns a percentage of distance violation. + +def collisions(agents, robot): + robot_coll_list = [0] * len(robot) + person_coll_list = [0] * len(robot) + robot_collisions = 0 + person_collisions = 0 + + for i in range(len(robot)): + for agent in agents[i].agents: + + if euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius < 0.02: + + # Robot's angle + nrx = (robot[i].position.position.x - agent.position.position.x) * math.cos(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.sin(agent.yaw) + nry = -(robot[i].position.position.x - agent.position.position.x) * math.sin(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.cos(agent.yaw) + alpha = math.atan2(nry, nrx) + + # Agent's angle + nrx = (agent.position.position.x - robot[i].position.position.x) * math.cos(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.sin(robot[i].yaw) + nry = -(agent.position.position.x - robot[i].position.position.x) * math.sin(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.cos(robot[i].yaw) + alpha2 = math.atan2(nrx, nry) + + if abs(alpha) < abs(alpha2) and robot[i].linear_vel > agent.linear_vel: + robot_collisions += 1 + robot_coll_list[i] = 1 + elif abs(alpha) > abs(alpha2) and robot[i].linear_vel < agent.linear_vel: + person_collisions += 1 + person_coll_list[i] = 1 + elif abs(alpha) < abs(alpha2) and robot[i].linear_vel < agent.linear_vel: + #person_collision += 1 + robot_collisions += 1 + robot_coll_list[i] = 1 + elif abs(alpha) > abs(alpha2) and robot[i].linear_vel > agent.linear_vel: + #robot_collision += 1 + person_collisions += 1 + person_coll_list[i] = 1 + elif abs(alpha) == abs(alpha2) and robot[i].linear_vel == agent.linear_vel: + robot_collisions += 1 + person_collisions += 1 + robot_coll_list[i] = 1 + person_coll_list[i] = 1 + + return robot_collisions, person_collisions, robot_coll_list, person_coll_list + +def robot_on_person_collision(agents, robot): + + collision = collisions(agents, robot) + print('Robot_on_person_collision: %i ' % collision[0]) + + return [collision[0], collision[2]] + +def person_on_robot_collision(agents, robot): + + collision = collisions(agents, robot) + print('Person_on_robot_collision: %i' % collision[1]) + + return [collision[1], collision[3]] + + +def time_not_moving(agents, robot): + + not_moving = [0]*len(robot) + time_step = total_time(agents, robot)[0]/len(agents) + + count = 0 + for index, r in enumerate(robot): + if(r.linear_vel < 0.01 and abs(r.angular_vel < 0.02)): + count=count+1 + not_moving[index]=1 + time_stopped = time_step*count + print('Time stopped: %i secs' % time_stopped) + return [time_stopped, not_moving] + + +def goal_reached(agents, robot): + mind = 0.0 + if(len(robot[-1].goals)): + for g in robot[-1].goals: + d = euclidean_distance(robot[-1].position, g) - robot[-1].goal_radius + if d Trajectory needed +# Final Displacement Error --> Trajectory needed +# Asymmetric Dynamic Time Warping --> Trajectory needed +# Topological Complexity --> Path needed +# Path irregularity and Path efficiency are similar to the ones in SEAN paper . +# Personal space and o/p/r-space metrics are similar to the ones in Teaching Robot Navigation Behaviors to Optimal RRT Planners paper. + + + + +metrics = { + # N. Perez-Higueras, F. Caballero, and L. Merino, “Teaching Robot Nav- + # igation Behaviors to Optimal RRT Planners,” International Journal of + # Social Robotics, vol. 10, no. 2, pp. 235–249, 2018. + 'time_to_reach_goal': total_time, + 'path_length': robot_path_length, + 'cumulative_heading_changes': cumulative_heading_changes, + 'avg_distance_to_closest_person': avg_closest_person, + 'minimum_distance_to_people': minimum_distance_to_people, + 'maximum_distance_to_people': maximum_distance_to_people, + 'intimate_space_intrusions': intimate_space_intrusions, + 'personal_space_intrusions': personal_space_intrusions, + 'social_space_intrusions': social_space_intrusions, + 'group_intimate_space_intrusions': group_intimate_space_intrusions, + 'group_personal_space_intrusions': group_personal_space_intrusions, + 'group_social_space_intrusions': group_social_space_intrusions, + # N. Tsoi, A. Xiang, P. Yu, S. S. Sohn, G. Schwartz, S. Ramesh, + # M. Hussein, A. W. Gupta, M. Kapadia, and M. V ́azquez, “Sean 2.0: + # Formalizing and generating social situations for robot navigation,” + # IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 11 047– + # 11 054, 2022 + # - 'Total Path Length' (meters): similar to 'path_length' + # - 'Path Irregularity': (radians): total rotations in the robot's + # traveled path greater than the total rotations in the search-based + # path from the starting pose. + # - 'Path Efficiency': (meters): ratio between robot's traveled path and + # geodesic distance of the search-based path from the starting pose. + + # true when the robot's final pose is within a specified distance of the goal. + # The final distance threshold is easily adjustable by the user, but defaults + # to 1.2m. + 'completed': goal_reached, + #(meters): the closest the robot passes to the target position. + 'minimum_distance_to_target': minimum_goal_distance, + #(meters): distance between the last robot position and the target position. + 'final_distance_to_target': final_goal_distance, + # - 'Robot on Person Personal Distance Violation': number of times a robot + # approaches a person within the personal distance of the robot. + # Similar to 'personal_space_intrusions' + # - 'Person on Robot Personal Distance Violation': number of times a person + # approaches the robot within the personal distance of the robot. + # - 'Intimate Distance Violation': number of times the robot approached within + # the intimate distance of a person. + # - 'Person on Robot Intimate Distance Violation': number of times a person + # approaches the robot within the intimate distance of the robot. + 'robot_on_person_collision': robot_on_person_collision, + 'person_on_robot_collision': person_on_robot_collision, + 'time_not_moving': time_not_moving, + # TODO: 'static_obstacle_collision': static_obs_collision, + # number of times the robot collides with a static obstacle. + + # SocNavBench: A Grounded Simulation Testing Framework for Evaluating Social Navigation + #ABHIJAT BISWAS, ALLAN WANG, GUSTAVO SILVERA, AARON STEINFELD, and HENNY AD-MONI, Carnegie Mellon University + 'avg_robot_linear_speed': avg_robot_linear_speed, + 'avg_robot_angular_speed': avg_robot_angular_speed, + 'avg_acceleration': avg_acceleration, + 'avg_overacceleration': avg_overacceleration, + + # Learning a Group-Aware Policy for Robot Navigation + # Kapil Katyal ∗1,2 , Yuxiang Gao ∗2 , Jared Markowitz 1 , Sara Pohland 3 , Corban Rivera 1 , I-Jeng Wang 1 , Chien-Ming Huang 2 + 'avg_pedestrian_velocity': avg_pedestrian_velocity, + 'avg_closest_pedestrian_velocity': avg_closest_pedestrian_velocity, + + # metrics based on Social Force Model employed in different papers + 'social_force_on_agents': social_force_on_agents, + 'social_force_on_robot': social_force_on_robot, + 'social_work': social_work, + 'obstacle_force_on_robot': obstacle_force_on_robot, + 'obstacle_force_on_agents': obstacle_force_on_agents, +} \ No newline at end of file From d87ed4981d80d2b46e3f316b944cd5b0ce61cec4 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 21 Feb 2024 15:41:46 +0900 Subject: [PATCH 07/11] update evaluation_metrics Signed-off-by: Masayuki Murata --- cabot_navigation2/test/evaluation_metrics.py | 287 +++++++++++-------- 1 file changed, 164 insertions(+), 123 deletions(-) diff --git a/cabot_navigation2/test/evaluation_metrics.py b/cabot_navigation2/test/evaluation_metrics.py index e2962bf1..54b173f0 100644 --- a/cabot_navigation2/test/evaluation_metrics.py +++ b/cabot_navigation2/test/evaluation_metrics.py @@ -25,16 +25,11 @@ # metrics implementation is based on Hunav Evaluator (MIT license) # https://github.com/robotics-upo/hunav_sim/tree/humble/hunav_evaluator -from importlib.resources import path -from tokenize import group import numpy as np import math -import sys import rclpy -from hunav_msgs.msg import Agents -from hunav_msgs.msg import Agent +from pedestrian_plugin_msgs.msg import Agent from geometry_msgs.msg import Pose -from hunav_evaluator.sfm import SFM # Teaching Robot Navigation Behaviors to Optimal RRT Planners # Noé Pérez-Higueras, Fernando Caballero & Luis Merino @@ -104,27 +99,35 @@ def total_time(agents, robot): t1 = rclpy.time.Time.from_msg(agents[0].header.stamp) dur = (t2 - t1).to_msg() secs = float(dur.sec + dur.nanosec/1e9) - print('\nTime_to_reach_goal computed: %.2f secs' % secs) return [secs] -def robot_path_length(agents, robot): +def robot_path_length(agents, robot, accumulation_threshold=0.01): path_length = 0.0 + prev_position = robot[0].position for i in range(len(robot)-1): - path_length += euclidean_distance(robot[i+1].position, robot[i].position) - print('Path_length computed: %.2f m' % path_length) + distance = euclidean_distance(robot[i+1].position, prev_position) + # ignore small changes + if accumulation_threshold <= distance: + path_length += distance + prev_position = robot[i+1].position return [path_length] -def cumulative_heading_changes(agents, robot): +def cumulative_heading_changes(agents, robot, accumulation_threshold=math.radians(1)): chc_list = [0.0] chc = 0 + prev_yaw = robot[0].yaw for i in range(len(robot) - 1): - norm = normalize_angle(robot[i].yaw - robot[i+1].yaw) + norm = normalize_angle(prev_yaw - robot[i+1].yaw) if norm < 0.0: norm *= -1 + # ignore small changes + if accumulation_threshold <= norm: + prev_yaw = robot[i+1].yaw + else: + norm = 0.0 chc += norm chc_list.append(norm) - print('Cumulative_heading_changes: %.2f rads' % chc) return [chc, chc_list] def normalize_angle(ang): @@ -150,7 +153,6 @@ def avg_closest_person(agents, robot): min_dist_list.append(min_dist) avg_dist = avg_dist/len(robot) - print('Average_closest_person: %.2f m' % avg_dist) return [avg_dist, min_dist_list] @@ -165,8 +167,6 @@ def minimum_distance_to_people(agents, robot): min_distance.append(d) min_dist = min(min_distance) - - print('Minimum_distance_to_people: %.2f m' % min_dist) return [min_dist] @@ -178,8 +178,6 @@ def maximum_distance_to_people(agents, robot): max_distance.append(euclidean_distance(robot[i].position, agent.position) - robot[i].radius) max_dist = max(max_distance) - - print('Maximum_distance_to_people: %.2f m' % max_dist) return [max_dist] @@ -209,22 +207,20 @@ def space_intrusions(agents, robot, k): def intimate_space_intrusions(agents, robot): - percentage, slist = space_intrusions(agents, robot, 'intimate') - print('Intimate_space_intrusions: %.2f %% of the total time' % percentage) + percentage, slist = space_intrusions(agents, robot, 'intimate') return [percentage, slist] + def personal_space_intrusions(agents, robot): - percentage, slist = space_intrusions(agents, robot, 'personal') - print('Personal_space_intrusions: %.2f %% of the total time' % percentage) + percentage, slist = space_intrusions(agents, robot, 'personal') return [percentage, slist] - + + def social_space_intrusions(agents, robot): - percentage, slist = space_intrusions(agents, robot, 'social') - print('Social_space_intrusions: %.2f %% of the total time' % percentage) + percentage, slist = space_intrusions(agents, robot, 'social') return [percentage, slist] - def detect_groups(agents): group_ids = [] for a in agents[0].agents: @@ -262,21 +258,17 @@ def group_space_intrusions(agents, robot, k): def group_intimate_space_intrusions(agents, robot): r = group_space_intrusions(agents, robot, 'intimate') - print('Group_intimate_space_intrusions: %.2f %% of the total time' % r[0]) return r + def group_personal_space_intrusions(agents, robot): - r = group_space_intrusions(agents, robot, 'personal') - print('Group_personal_space_intrusions: %.2f %% of the total time' % r[0]) + r = group_space_intrusions(agents, robot, 'personal') return r -def group_social_space_intrusions(agents, robot): - r = group_space_intrusions(agents, robot, 'social') - print('Group_social_space_intrusions: %.2f %% of the total time' % r[0]) - return r - - +def group_social_space_intrusions(agents, robot): + r = group_space_intrusions(agents, robot, 'social') + return r # SEAN 2.0: Formalizing and Generating Social Situations for Robot Navigation @@ -296,7 +288,7 @@ def collisions(agents, robot): for agent in agents[i].agents: if euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius < 0.02: - + # Robot's angle nrx = (robot[i].position.position.x - agent.position.position.x) * math.cos(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.sin(agent.yaw) nry = -(robot[i].position.position.x - agent.position.position.x) * math.sin(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.cos(agent.yaw) @@ -329,33 +321,91 @@ def collisions(agents, robot): return robot_collisions, person_collisions, robot_coll_list, person_coll_list -def robot_on_person_collision(agents, robot): +def robot_on_person_collision(agents, robot): collision = collisions(agents, robot) - print('Robot_on_person_collision: %i ' % collision[0]) - return [collision[0], collision[2]] + def person_on_robot_collision(agents, robot): - collision = collisions(agents, robot) - print('Person_on_robot_collision: %i' % collision[1]) + return [collision[1], collision[3]] + + +def collision_count(agents, robot): + robot_coll_list = [0] * len(robot) + person_coll_list = [0] * len(robot) + robot_collisions = 0 + person_collisions = 0 + + agents_in_collision = {} + + for i in range(len(robot)): + for agent in agents[i].agents: + agent_name = agent.name + + if euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius < 0.02: + in_collision = agents_in_collision.get(agent_name, False) + + # Robot's angle + nrx = (robot[i].position.position.x - agent.position.position.x) * math.cos(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.sin(agent.yaw) + nry = -(robot[i].position.position.x - agent.position.position.x) * math.sin(agent.yaw) + (robot[i].position.position.y - agent.position.position.y) * math.cos(agent.yaw) + alpha = math.atan2(nry, nrx) + + # Agent's angle + nrx = (agent.position.position.x - robot[i].position.position.x) * math.cos(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.sin(robot[i].yaw) + nry = -(agent.position.position.x - robot[i].position.position.x) * math.sin(robot[i].yaw) + (agent.position.position.y - robot[i].position.position.y) * math.cos(robot[i].yaw) + alpha2 = math.atan2(nrx, nry) + + if not in_collision: + if abs(alpha) < abs(alpha2) and robot[i].linear_vel > agent.linear_vel: + robot_collisions += 1 + robot_coll_list[i] += 1 + elif abs(alpha) > abs(alpha2) and robot[i].linear_vel < agent.linear_vel: + person_collisions += 1 + person_coll_list[i] += 1 + elif abs(alpha) < abs(alpha2) and robot[i].linear_vel < agent.linear_vel: + # person_collision += 1 + robot_collisions += 1 + robot_coll_list[i] += 1 + elif abs(alpha) > abs(alpha2) and robot[i].linear_vel > agent.linear_vel: + # robot_collision += 1 + person_collisions += 1 + person_coll_list[i] += 1 + elif abs(alpha) == abs(alpha2) and robot[i].linear_vel == agent.linear_vel: + robot_collisions += 1 + person_collisions += 1 + robot_coll_list[i] += 1 + person_coll_list[i] += 1 + + agents_in_collision[agent_name] = True + else: + agents_in_collision[agent_name] = False + return robot_collisions, person_collisions, robot_coll_list, person_coll_list + + +def robot_on_person_collision_count(agents, robot): + collision = collision_count(agents, robot) + return [collision[0], collision[2]] + + +def person_on_robot_collision_count(agents, robot): + collision = collision_count(agents, robot) return [collision[1], collision[3]] -def time_not_moving(agents, robot): +def time_not_moving(agents, robot, linear_vel_threshold=0.01, angular_vel_threshold=0.02): not_moving = [0]*len(robot) time_step = total_time(agents, robot)[0]/len(agents) count = 0 for index, r in enumerate(robot): - if(r.linear_vel < 0.01 and abs(r.angular_vel < 0.02)): + if(r.linear_vel < linear_vel_threshold and abs(r.angular_vel < angular_vel_threshold)): count=count+1 not_moving[index]=1 time_stopped = time_step*count - print('Time stopped: %i secs' % time_stopped) return [time_stopped, not_moving] @@ -392,7 +442,7 @@ def minimum_goal_distance(agents, robot): # SocNavBench: A Grounded Simulation Testing Framework for Evaluating Social Navigation -#ABHIJAT BISWAS, ALLAN WANG, GUSTAVO SILVERA, AARON STEINFELD, and HENNY AD-MONI, Carnegie Mellon University +# ABHIJAT BISWAS, ALLAN WANG, GUSTAVO SILVERA, AARON STEINFELD, and HENNY AD-MONI, Carnegie Mellon University def avg_robot_linear_speed(agents, robot): speed_list = [] @@ -403,9 +453,9 @@ def avg_robot_linear_speed(agents, robot): speed = speed / len(robot) - print('Average_robot_speed: %.2f m/s' % speed) return [speed, speed_list] + def avg_robot_angular_speed(agents, robot): speed_list = [] speed = 0 @@ -415,11 +465,9 @@ def avg_robot_angular_speed(agents, robot): speed = speed / len(robot) - print('Average_robot_speed: %.2f rad/s' % speed) return [speed, speed_list] - def avg_acceleration(agents, robot): acceleration = 0 acceleration_list = [0.0] @@ -438,8 +486,6 @@ def avg_acceleration(agents, robot): acceleration = acceleration / len(robot) - print('Average_robot_acceleration: %.5f m/s^2' % acceleration) - return [acceleration, acceleration_list] @@ -461,8 +507,6 @@ def avg_overacceleration(agents, robot): jerk = jerk / len(robot) - print('Average_robot_jerk(over_acceleration): %.5f m/s^3' % jerk) - return [jerk, jerk_list] @@ -480,8 +524,6 @@ def avg_pedestrian_velocity(agents, robot): speed_list.append(speed2/len(agents[i].agents)) speed = speed / (len(agents) * len(agents[0].agents)) - - print('Average_Pedestrian_speed: %.2f m/s' % speed) return [speed, speed_list] @@ -502,9 +544,8 @@ def avg_closest_pedestrian_velocity(agents, robot): speed += closest.linear_vel speed_list.append(closest.linear_vel) - + speed = speed/len(robot) - print('Speed average closest person: %.2f m/s' % speed) return [speed, speed_list] @@ -515,68 +556,68 @@ def avg_pedestrian_angle(agents, robot): # metrics based on social force model -# cumulative modulus of the social force provoked -# by the robot in the agents -def social_force_on_agents(agents, robot): - sfm = SFM() - sf = 0.0 - sf_list = [] - for agts, rb in zip(agents, robot): - f = sfm.modulusSocialForce2(rb, agts) - sf += f - sf_list.append(f) - return [sf, sf_list] - -# cumulative modulus of the social force provoked -# by the agents in the robot -def social_force_on_robot(agents, robot): - sfm = SFM() - sf = 0.0 - sf_list = [] - for agts, rb in zip(agents, robot): - f = sfm.modulusSocialForce(rb, agts) - sf += f - sf_list.append(f) - return [sf, sf_list] - -# cumulative social work employed in this planner: -# https://github.com/robotics-upo/social_force_window_planner -def social_work(agents, robot): - sfm = SFM() - sw = 0.0 - sw_list = [] - for agts, rb in zip(agents, robot): - f = sfm.computeSocialWork(rb, agts) - sw += f - sw_list.append(f) - return [sw, sw_list] - - -# cumulative obstacle force on the agents -def obstacle_force_on_agents(agents, robot): - sfm = SFM() - of = 0.0 - of_list = [] - for agts in agents: - avg = 0.0 - for a in agts.agents: - avg += np.linalg.norm(sfm.computeObstacleForce(a)) - avg =avg/len(agts.agents) - of += avg - of_list.append(avg) - return [of, of_list] - - -# cumulative obstacle force on the robot -def obstacle_force_on_robot(agents, robot): - sfm = SFM() - of = 0.0 - of_list = [] - for r in robot: - f = np.linalg.norm(sfm.computeObstacleForce(r)) - of += f - of_list.append(f) - return [of, of_list] +# # cumulative modulus of the social force provoked +# # by the robot in the agents +# def social_force_on_agents(agents, robot): +# sfm = SFM() +# sf = 0.0 +# sf_list = [] +# for agts, rb in zip(agents, robot): +# f = sfm.modulusSocialForce2(rb, agts) +# sf += f +# sf_list.append(f) +# return [sf, sf_list] + +# # cumulative modulus of the social force provoked +# # by the agents in the robot +# def social_force_on_robot(agents, robot): +# sfm = SFM() +# sf = 0.0 +# sf_list = [] +# for agts, rb in zip(agents, robot): +# f = sfm.modulusSocialForce(rb, agts) +# sf += f +# sf_list.append(f) +# return [sf, sf_list] + +# # cumulative social work employed in this planner: +# # https://github.com/robotics-upo/social_force_window_planner +# def social_work(agents, robot): +# sfm = SFM() +# sw = 0.0 +# sw_list = [] +# for agts, rb in zip(agents, robot): +# f = sfm.computeSocialWork(rb, agts) +# sw += f +# sw_list.append(f) +# return [sw, sw_list] + + +# # cumulative obstacle force on the agents +# def obstacle_force_on_agents(agents, robot): +# sfm = SFM() +# of = 0.0 +# of_list = [] +# for agts in agents: +# avg = 0.0 +# for a in agts.agents: +# avg += np.linalg.norm(sfm.computeObstacleForce(a)) +# avg =avg/len(agts.agents) +# of += avg +# of_list.append(avg) +# return [of, of_list] + + +# # cumulative obstacle force on the robot +# def obstacle_force_on_robot(agents, robot): +# sfm = SFM() +# of = 0.0 +# of_list = [] +# for r in robot: +# f = np.linalg.norm(sfm.computeObstacleForce(r)) +# of += f +# of_list.append(f) +# return [of, of_list] #TODO @@ -674,9 +715,9 @@ def static_obs_collision(agents, robot): 'avg_closest_pedestrian_velocity': avg_closest_pedestrian_velocity, # metrics based on Social Force Model employed in different papers - 'social_force_on_agents': social_force_on_agents, - 'social_force_on_robot': social_force_on_robot, - 'social_work': social_work, - 'obstacle_force_on_robot': obstacle_force_on_robot, - 'obstacle_force_on_agents': obstacle_force_on_agents, + # 'social_force_on_agents': social_force_on_agents, + # 'social_force_on_robot': social_force_on_robot, + # 'social_work': social_work, + # 'obstacle_force_on_robot': obstacle_force_on_robot, + # 'obstacle_force_on_agents': obstacle_force_on_agents, } \ No newline at end of file From d1a003b32af0a0996bb608714e1db5f53a3fe644 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 21 Feb 2024 19:03:57 +0900 Subject: [PATCH 08/11] update formatting of evaluation metrics Signed-off-by: Masayuki Murata --- cabot_navigation2/test/evaluation_metrics.py | 125 +++++++++---------- 1 file changed, 60 insertions(+), 65 deletions(-) diff --git a/cabot_navigation2/test/evaluation_metrics.py b/cabot_navigation2/test/evaluation_metrics.py index 54b173f0..b77dd308 100644 --- a/cabot_navigation2/test/evaluation_metrics.py +++ b/cabot_navigation2/test/evaluation_metrics.py @@ -34,12 +34,13 @@ # Teaching Robot Navigation Behaviors to Optimal RRT Planners # Noé Pérez-Higueras, Fernando Caballero & Luis Merino + def euclidean_distance(pose, pose1): return math.sqrt((pose.position.x - pose1.position.x)**2 + (pose.position.y - pose1.position.y)**2) def get_group_center(agents_i, group_id, distance): - + group = [] for agent in agents_i: if agent.group_id == group_id: @@ -48,17 +49,16 @@ def get_group_center(agents_i, group_id, distance): pose.position.y = agent.position.position.y + (distance * math.sin(agent.yaw)) group.append(pose) - interaction_center = Pose() + interaction_center = Pose() for p in group: interaction_center.position.x += p.position.x interaction_center.position.y += p.position.y - + interaction_center.position.x = float(interaction_center.position.x/len(group)) interaction_center.position.y = float(interaction_center.position.y/len(group)) return interaction_center - def indicator_function(norm, k): if k == 'intimate': if norm < 0.45: @@ -82,18 +82,19 @@ def indicator_function(norm, k): return 0 else: return 0 - + def get_time_stamps(agents, robot): time_list = [] t0 = rclpy.time.Time.from_msg(agents[0].header.stamp) - for a in agents: + for a in agents: t = rclpy.time.Time.from_msg(a.header.stamp) dur = (t - t0).to_msg() s = float(dur.sec + dur.nanosec/1e9) time_list.append(s) return time_list + def total_time(agents, robot): t2 = rclpy.time.Time.from_msg(agents[len(agents)-1].header.stamp) t1 = rclpy.time.Time.from_msg(agents[0].header.stamp) @@ -101,6 +102,7 @@ def total_time(agents, robot): secs = float(dur.sec + dur.nanosec/1e9) return [secs] + def robot_path_length(agents, robot, accumulation_threshold=0.01): path_length = 0.0 prev_position = robot[0].position @@ -112,6 +114,7 @@ def robot_path_length(agents, robot, accumulation_threshold=0.01): prev_position = robot[i+1].position return [path_length] + def cumulative_heading_changes(agents, robot, accumulation_threshold=math.radians(1)): chc_list = [0.0] chc = 0 @@ -130,25 +133,27 @@ def cumulative_heading_changes(agents, robot, accumulation_threshold=math.radian return [chc, chc_list] + def normalize_angle(ang): - while (ang <= -math.pi): - ang += 2 * math.pi + while (ang <= -math.pi): + ang += 2 * math.pi while (ang > math.pi): - ang -= 2 * math.pi + ang -= 2 * math.pi return ang + def avg_closest_person(agents, robot): min_dist_list = [] avg_dist = 0 for i in range(len(robot)): - min_dist = 10000 + min_dist = 10000 for agent in agents[i].agents: d = euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius - if(d < min_dist): + if (d < min_dist): min_dist = d if min_dist < 0.0: min_dist = 0.0 - if(len(agents[i].agents)>0): + if (len(agents[i].agents) > 0): avg_dist += min_dist min_dist_list.append(min_dist) @@ -158,31 +163,25 @@ def avg_closest_person(agents, robot): def minimum_distance_to_people(agents, robot): min_distance = list() - for i in range(len(robot)): for agent in agents[i].agents: d = euclidean_distance(robot[i].position, agent.position) - robot[i].radius - agent.radius - if d<0.0: + if d < 0.0: d = 0.0 - min_distance.append(d) - + min_distance.append(d) min_dist = min(min_distance) - return [min_dist] + def maximum_distance_to_people(agents, robot): max_distance = list() - for i in range(len(robot)): for agent in agents[i].agents: max_distance.append(euclidean_distance(robot[i].position, agent.position) - robot[i].radius) - max_dist = max(max_distance) - return [max_dist] - def space_intrusions(agents, robot, k): space_intrusions = 0 space_intrusions_list = [0] * len(robot) @@ -198,7 +197,7 @@ def space_intrusions(agents, robot, k): indicator = indicator_function(min_dist, k) if indicator == 1: space_intrusions += 1 - space_intrusions_list[i]=1 + space_intrusions_list[i] = 1 space_intrusions = space_intrusions / len(robot) percentage = space_intrusions * 100.0 @@ -224,7 +223,7 @@ def social_space_intrusions(agents, robot): def detect_groups(agents): group_ids = [] for a in agents[0].agents: - if(a.group_id != -1 and ((a.group_id in group_ids) == False)): + if (a.group_id != -1 and ((a.group_id in group_ids) == False)): group_ids.append(a.group_id) return group_ids @@ -232,10 +231,10 @@ def detect_groups(agents): def group_space_intrusions(agents, robot, k): group_ids = detect_groups(agents) - if(len(group_ids)==0): + if (len(group_ids) == 0): return [0.0] - d=1.5 + d = 1.5 space_intrusions = 0 group_list = [0] * len(robot) for i in range(len(robot)): @@ -306,11 +305,11 @@ def collisions(agents, robot): person_collisions += 1 person_coll_list[i] = 1 elif abs(alpha) < abs(alpha2) and robot[i].linear_vel < agent.linear_vel: - #person_collision += 1 + # person_collision += 1 robot_collisions += 1 robot_coll_list[i] = 1 elif abs(alpha) > abs(alpha2) and robot[i].linear_vel > agent.linear_vel: - #robot_collision += 1 + # robot_collision += 1 person_collisions += 1 person_coll_list[i] = 1 elif abs(alpha) == abs(alpha2) and robot[i].linear_vel == agent.linear_vel: @@ -411,32 +410,31 @@ def time_not_moving(agents, robot, linear_vel_threshold=0.01, angular_vel_thresh def goal_reached(agents, robot): mind = 0.0 - if(len(robot[-1].goals)): + if (len(robot[-1].goals)): for g in robot[-1].goals: d = euclidean_distance(robot[-1].position, g) - robot[-1].goal_radius if d Trajectory needed # Final Displacement Error --> Trajectory needed # Asymmetric Dynamic Time Warping --> Trajectory needed @@ -649,8 +646,6 @@ def static_obs_collision(agents, robot): # Personal space and o/p/r-space metrics are similar to the ones in Teaching Robot Navigation Behaviors to Optimal RRT Planners paper. - - metrics = { # N. Perez-Higueras, F. Caballero, and L. Merino, “Teaching Robot Nav- # igation Behaviors to Optimal RRT Planners,” International Journal of @@ -673,28 +668,28 @@ def static_obs_collision(agents, robot): # IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 11 047– # 11 054, 2022 # - 'Total Path Length' (meters): similar to 'path_length' - # - 'Path Irregularity': (radians): total rotations in the robot's - # traveled path greater than the total rotations in the search-based + # - 'Path Irregularity': (radians): total rotations in the robot's + # traveled path greater than the total rotations in the search-based # path from the starting pose. - # - 'Path Efficiency': (meters): ratio between robot's traveled path and + # - 'Path Efficiency': (meters): ratio between robot's traveled path and # geodesic distance of the search-based path from the starting pose. - # true when the robot's final pose is within a specified distance of the goal. - # The final distance threshold is easily adjustable by the user, but defaults + # true when the robot's final pose is within a specified distance of the goal. + # The final distance threshold is easily adjustable by the user, but defaults # to 1.2m. 'completed': goal_reached, - #(meters): the closest the robot passes to the target position. - 'minimum_distance_to_target': minimum_goal_distance, - #(meters): distance between the last robot position and the target position. - 'final_distance_to_target': final_goal_distance, - # - 'Robot on Person Personal Distance Violation': number of times a robot + # (meters): the closest the robot passes to the target position. + 'minimum_distance_to_target': minimum_goal_distance, + # (meters): distance between the last robot position and the target position. + 'final_distance_to_target': final_goal_distance, + # - 'Robot on Person Personal Distance Violation': number of times a robot # approaches a person within the personal distance of the robot. # Similar to 'personal_space_intrusions' - # - 'Person on Robot Personal Distance Violation': number of times a person + # - 'Person on Robot Personal Distance Violation': number of times a person # approaches the robot within the personal distance of the robot. - # - 'Intimate Distance Violation': number of times the robot approached within + # - 'Intimate Distance Violation': number of times the robot approached within # the intimate distance of a person. - # - 'Person on Robot Intimate Distance Violation': number of times a person + # - 'Person on Robot Intimate Distance Violation': number of times a person # approaches the robot within the intimate distance of the robot. 'robot_on_person_collision': robot_on_person_collision, 'person_on_robot_collision': person_on_robot_collision, @@ -703,7 +698,7 @@ def static_obs_collision(agents, robot): # number of times the robot collides with a static obstacle. # SocNavBench: A Grounded Simulation Testing Framework for Evaluating Social Navigation - #ABHIJAT BISWAS, ALLAN WANG, GUSTAVO SILVERA, AARON STEINFELD, and HENNY AD-MONI, Carnegie Mellon University + # ABHIJAT BISWAS, ALLAN WANG, GUSTAVO SILVERA, AARON STEINFELD, and HENNY AD-MONI, Carnegie Mellon University 'avg_robot_linear_speed': avg_robot_linear_speed, 'avg_robot_angular_speed': avg_robot_angular_speed, 'avg_acceleration': avg_acceleration, @@ -720,4 +715,4 @@ def static_obs_collision(agents, robot): # 'social_work': social_work, # 'obstacle_force_on_robot': obstacle_force_on_robot, # 'obstacle_force_on_agents': obstacle_force_on_agents, -} \ No newline at end of file +} From fb8806be55df4ba7db68f1dab6484d5e5511648e Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Wed, 21 Feb 2024 02:49:11 +0900 Subject: [PATCH 09/11] add initial implementation of metric evaluator for cabot_navigation2 test Signed-off-by: Masayuki Murata --- cabot_navigation2/test/evaluation_metrics.py | 27 ++-- cabot_navigation2/test/evaluator.py | 131 +++++++++++++++++++ cabot_navigation2/test/run_test.py | 23 ++++ 3 files changed, 172 insertions(+), 9 deletions(-) create mode 100644 cabot_navigation2/test/evaluator.py diff --git a/cabot_navigation2/test/evaluation_metrics.py b/cabot_navigation2/test/evaluation_metrics.py index b77dd308..d477775f 100644 --- a/cabot_navigation2/test/evaluation_metrics.py +++ b/cabot_navigation2/test/evaluation_metrics.py @@ -395,16 +395,25 @@ def person_on_robot_collision_count(agents, robot): def time_not_moving(agents, robot, linear_vel_threshold=0.01, angular_vel_threshold=0.02): - + time_stopped = 0 not_moving = [0]*len(robot) - time_step = total_time(agents, robot)[0]/len(agents) - - count = 0 - for index, r in enumerate(robot): - if(r.linear_vel < linear_vel_threshold and abs(r.angular_vel < angular_vel_threshold)): - count=count+1 - not_moving[index]=1 - time_stopped = time_step*count + + stamps = get_time_stamps(agents, robot) + + for i in range(len(robot)): + r = robot[i] + + if i == 0: + dt = (stamps[i+1] - stamps[i])/2.0 + elif i == len(robot)-1: + dt = (stamps[i] - stamps[i-1])/2.0 + else: + dt = (stamps[i+1] - stamps[i])/2.0 + (stamps[i] - stamps[i-1])/2.0 + + if (r.linear_vel < linear_vel_threshold and abs(r.angular_vel < angular_vel_threshold)): + time_stopped += dt + not_moving[i] = 1 + return [time_stopped, not_moving] diff --git a/cabot_navigation2/test/evaluator.py b/cabot_navigation2/test/evaluator.py new file mode 100644 index 00000000..b0e8ce49 --- /dev/null +++ b/cabot_navigation2/test/evaluator.py @@ -0,0 +1,131 @@ +#!/usr/bin/env python3 + +############################################################################### +# Copyright (c) 2024 IBM Corporation +# +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: +# +# The above copyright notice and this permission notice shall be included in +# all copies or substantial portions of the Software. +# +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +# THE SOFTWARE. +############################################################################### + +import message_filters +from dataclasses import dataclass, field +from typing import Optional +import evaluation_metrics +from pedestrian_plugin_msgs.msg import Agent, Agents, Metric + + +@dataclass +class EvaluationParameter(): + metrics: Optional[list] = field(default_factory=list) + robot_radius: Optional[float] = None + + +class Evaluator: + def __init__(self, node): + self.node = node + + # variables + self.robot_list = [] + self.human_list = [] + self._recording = False + self._ready = False + self._logger = None + self._metrics_to_compute = [] + self._metrics_func_list = [] + + # self evaluation parameters + self._robot_radius = None + + # subscriber and publisher + self.robot_sub = message_filters.Subscriber(node, Agent, "/robot_states") + self.human_sub = message_filters.Subscriber(node, Agents, "/human_states") + self.metric_pub = self.node.create_publisher(Metric, "/metric", 10) + self.time_synchronizer = message_filters.TimeSynchronizer([self.robot_sub, self.human_sub], 10) + self.time_synchronizer.registerCallback(self.agents_callback) + + # timer + self._timer_period_sec = 1.0 # [s] + self._evaluation_timer = None + + def set_logger(self, logger): + self._logger = logger + + def set_timer_period(self, timer_period_sec: float): + self._timer_period_sec = timer_period_sec + + def set_metrics_to_compute(self, metrics_to_compute): + self._metrics_to_compute = metrics_to_compute + self._metrics_func_list = [getattr(evaluation_metrics, metric) for metric in metrics_to_compute] + + def set_evaluation_parameters(self, **kwargs): + eval_params = EvaluationParameter(**kwargs) + self.set_metrics_to_compute(eval_params.metrics) + if eval_params.robot_radius is not None: + self._robot_radius = float(eval_params.robot_radius) + self._logger.debug(f"evaluation_parameters={eval_params}") + + def start(self): + self.reset() + self._recording = True + if self._evaluation_timer is None: + self._evaluation_timer = self.node.create_timer(self._timer_period_sec, self.run_evaluation) + + def stop(self): + self._recording = False + self.run_evaluation() + if self._evaluation_timer is not None: + self._evaluation_timer.cancel() + self._evaluation_timer.destroy() + self._evaluation_timer = None + self.reset() + + def reset(self): + self.robot_list = [] + self.human_list = [] + self._ready = False + + def agents_callback(self, robot, human): + if self._recording: + # robot + if self._robot_radius is not None: + robot.radius = self._robot_radius + + self.robot_list.append(robot) + # record only active human + humans_active = [a for a in human.agents if a.behavior_state == Agent.ACTIVE] + human.agents = humans_active + self.human_list.append(human) + + if 2 <= len(self.robot_list): + self._ready = True + + def run_evaluation(self): + if not self._ready: + return + + for metric, func in zip(self._metrics_to_compute, self._metrics_func_list): + result = func(self.human_list, self.robot_list) + value = result[0] + + if self._logger is not None: + self._logger.debug(f"{metric}={value}") + + msg = Metric() + msg.name = metric + msg.value = float(value) + self.metric_pub.publish(msg) diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index 47e238e8..7e5247e3 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -46,6 +46,8 @@ from pedestrian.manager import PedestrianManager +# cabot_navigation2/test +from evaluator import Evaluator def import_class(input_str): # Split the input string and form module and class strings @@ -109,6 +111,11 @@ def __init__(self, node): self.set_entity_state_client = self.node.create_client(SetEntityState, '/gazebo/set_entity_state') self.test_func_name = None self.result = {} + # evaluation + self.evaluator = None + + def set_evaluator(self, evaluator): + self.evaluator = evaluator def test(self, module, specific_test, wait_ready=False): functions = [func for func in dir(module) if inspect.isfunction(getattr(module, func))] @@ -134,6 +141,7 @@ def test(self, module, specific_test, wait_ready=False): logger.info(f"Testing {specific_test}") self.test_func_name = specific_test getattr(module, specific_test)(self) + self.stop_evaluation() # automatically stop metric evaluation success = self.print_result(self.result, specific_test) self.cancel_subscription(specific_test) else: @@ -143,6 +151,7 @@ def test(self, module, specific_test, wait_ready=False): logger.info(f"Testing {func}") self.test_func_name = func getattr(module, func)(self) + self.stop_evaluation() # automatically stop metric evaluation success = self.print_result(self.result, func) self.cancel_subscription(func) @@ -228,6 +237,16 @@ def default_config(self): def info(self, text): logger.info(text) + # evaluation + def set_evaluation_parameters(self, **kwargs): + self.evaluator.set_evaluation_parameters(**kwargs) + + def start_evaluation(self): + self.evaluator.start() + + def stop_evaluation(self): + self.evaluator.stop() + # shorthand functions def check_collision(self, **kwargs): return self.check_topic_error(**dict( @@ -608,7 +627,11 @@ def main(): node = rclpy.node.Node("test_node") manager = PedestrianManager(node) + evaluator = Evaluator(node) + evaluator.set_logger(logger) + tester = Tester(node) + tester.set_evaluator(evaluator) mod = importlib.import_module(options.module) tester.test(mod, options.func, wait_ready=options.wait_ready) From 99fda33fdf8c48243a4641c6f9a01f5f82c86949 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Thu, 22 Feb 2024 01:43:00 +0900 Subject: [PATCH 10/11] update the pedestrian plugin to load robot_radius from pedestrian modules Signed-off-by: Masayuki Murata --- pedestrian_plugin/src/pedestrian_plugin.cpp | 27 +++++++++++++-------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/pedestrian_plugin/src/pedestrian_plugin.cpp b/pedestrian_plugin/src/pedestrian_plugin.cpp index 6887afa3..5de1ec6d 100644 --- a/pedestrian_plugin/src/pedestrian_plugin.cpp +++ b/pedestrian_plugin/src/pedestrian_plugin.cpp @@ -186,7 +186,18 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) PyObject* pArgs = PyTuple_New(0); PyObject* pDict = PyDict_New(); + // add plugin parameters to the arguments + for (const auto& pair : this->plugin_params) { + auto value = pair.second; + PyObject *temp = value.python_object(); + PyDict_SetItemString(pDict, pair.first.c_str(), temp); + Py_DECREF(temp); + } + + // set robot values to pDict if (robotModel) { + auto robot_radius = PythonUtils::getDictItemAsDouble(pDict, "robot_radius", 0.45); + PyObject* pRobotPose = PyDict_New(); ignition::math::Vector3d rPos = robotModel->WorldPose().Pos(); ignition::math::Quaterniond rRot = robotModel->WorldPose().Rot(); @@ -219,7 +230,7 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) pedestrian_plugin_msgs::msg::Agent robotAgent; robotAgent.type = pedestrian_plugin_msgs::msg::Agent::ROBOT; robotAgent.behavior_state = pedestrian_plugin_msgs::msg::Agent::ACTIVE; - robotAgent.name = std::string("robot"); + robotAgent.name = robotModel->GetName(); robotAgent.position = robot_pose; robotAgent.yaw = rRpy.Z(); robotAgent.velocity.linear.x = vel_x; @@ -227,12 +238,14 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) robotAgent.velocity.angular.z = vel_theta; robotAgent.linear_vel = vel_linear; robotAgent.angular_vel = vel_theta; - robotAgent.radius = 0.45; // default robot raduis + robotAgent.radius = robot_radius; // default robot raduis manager.updateRobotAgent(robotAgent); PyDict_SetItemString(pDict, "robot", pRobotPose); Py_DECREF(pRobotPose); } + + // set actor values to pDict PythonUtils::setDictItemAsFloat(pDict, "time", _info.simTime.Float()); PythonUtils::setDictItemAsFloat(pDict, "dt", dt); PythonUtils::setDictItemAsFloat(pDict, "x", this->x); @@ -241,20 +254,14 @@ void PedestrianPlugin::OnUpdate(const common::UpdateInfo &_info) PythonUtils::setDictItemAsFloat(pDict, "roll", this->roll); PythonUtils::setDictItemAsFloat(pDict, "pitch", this->pitch); PythonUtils::setDictItemAsFloat(pDict, "yaw", this->yaw); - - // add parameter to the arguments - for (const auto& pair : this->plugin_params) { - auto value = pair.second; - PyObject *temp = value.python_object(); - PyDict_SetItemString(pDict, pair.first.c_str(), temp); - Py_DECREF(temp); - } PyObject *aname = PyUnicode_DecodeFSDefault(this->actor->GetName().c_str()); PyDict_SetItemString(pDict, "name", aname); Py_DECREF(aname); + // call onUpdate in the python module auto pRet = PyObject_Call(pFunc, pArgs, pDict); + if (pRet != NULL && PyDict_Check(pRet)) { auto newX = PythonUtils::getDictItemAsDouble(pRet, "x", 0.0); auto newY = PythonUtils::getDictItemAsDouble(pRet, "y", 0.0); From e7bc047e3d712352265916ec7fdb7699ad364b87 Mon Sep 17 00:00:00 2001 From: Masayuki Murata Date: Fri, 23 Feb 2024 00:13:21 +0900 Subject: [PATCH 11/11] update cabot_navigation2/test/README.md and run_test.py to add description of metric evaluation Signed-off-by: Masayuki Murata --- cabot_navigation2/test/README.md | 16 ++++++++++++++++ cabot_navigation2/test/run_test.py | 25 +++++++++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/cabot_navigation2/test/README.md b/cabot_navigation2/test/README.md index 34cdb887..b2d7e08e 100644 --- a/cabot_navigation2/test/README.md +++ b/cabot_navigation2/test/README.md @@ -77,6 +77,22 @@ func test_name(tester: Tester) - `condition` : pyton snipet to check if the `msg` matches the condition - `wait` : wait for seconds - `seconds` : wait time in seconds +- `set_evaluation_parameters(metrics=[], robot_radius=None)` : set parameters used for computing metrics + - `metrics : Optional[list], default=[]` : list of metric functions to be computed. The callable functions are defined in [evaluation_metrics.py](evaluation_metrics.py). + - Example + ``` + metrics=[ + "total_time", + "robot_path_length", + "time_not_moving", + "avg_robot_linear_speed", + "cumulative_heading_changes" + ] + ``` + - Note: The implementation of the metric functions is not stable as it is under development. + - `robot_radius : Optional[float], default=None` : robot radius used to detect collisions in metric computation. If not defined, the default value (0.45) defined in the pedestrian plugin will be used. +- `start_evaluation` : start computing the metrics. This method should be called when ready to start the navigation. +- `stop_evaluation` : stop computing the metrics. It is usually not necessary to call this method because it is automatically called when the test ends. --- **old one with run_test_yaml.py** diff --git a/cabot_navigation2/test/run_test.py b/cabot_navigation2/test/run_test.py index 7e5247e3..21a3a111 100755 --- a/cabot_navigation2/test/run_test.py +++ b/cabot_navigation2/test/run_test.py @@ -239,12 +239,37 @@ def info(self, text): # evaluation def set_evaluation_parameters(self, **kwargs): + """ + Set parameters used for computing metrics + + Parameters are defined as EvaluationParameter dataclass in evaluator module + + Parameters + ---------- + metrics: Optional[list] = [] + List of metric functions to be computed. The callable functions are defined in evaluation_metrics.py + + robot_radius: Optional[float] = None + The robot radius used to detect collisions in the metric computation. + If not defined, the default value (0.45) defined in the pedestrian plugin is used. + """ self.evaluator.set_evaluation_parameters(**kwargs) def start_evaluation(self): + """ + Start computing the metrics + + This method should be called when ready to start the navigation + """ self.evaluator.start() def stop_evaluation(self): + """ + Stop comuting the metrics + + It is usually not necessary to call this method because it is automatically called when the test ends. + This method can be used when the user intentionally stops the metric computation + """ self.evaluator.stop() # shorthand functions