update odom subscriber to others

phatcvo · Jan 22, 2024 · 2af9ab1 · 2af9ab1
1 parent 0cf6409
commit 2af9ab1
Show file tree

Hide file tree

Showing 7 changed files with 44 additions and 41 deletions.
diff --git a/Schedule.md b/Schedule.md
@@ -28,7 +28,8 @@ TBD
   - [x] Output a one-point path as the result.
 
 - Local planner [P0]
-  - [ ] Subscribe to others' odom.
+  - [x] Subscribe to others' odom.
+    - [x] Transfer `agent_num` and `agent_id` from `start_robots.launch.xml` to `move_base.launch.xml`.
   - [ ] Calculate new velocities and publish them.
 
 - Bugs

diff --git a/src/core/local_planner/orca_planner/src/orca_planner.cpp b/src/core/local_planner/orca_planner/src/orca_planner.cpp
@@ -33,12 +33,13 @@ void OrcaPlanner::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::
     nh.param("agent_number", agent_number_, -1);
     nh.param("agent_id", agent_id_, -1);
 
+    other_odoms_.resize(agent_number_);
     for (int i = 0; i < agent_number_; ++i)
     {
       ros::Subscriber odom_sub = nh.subscribe<nav_msgs::Odometry>(
-          "/robot" + std::to_string(i) + "/odom", 1, boost::bind(&OrcaPlanner::odometryCallback, this, _1, i));
+          "/robot" + std::to_string(i + 1) + "/odom", 1, boost::bind(&OrcaPlanner::odometryCallback, this, _1, i + 1));
       odom_subs_.push_back(odom_sub);
-      ROS_WARN("agent %d, subscribe to agent %d.", agent_id_, i);
+      ROS_WARN("agent %d, subscribe to agent %d.", agent_id_, i + 1);
     }
   }
 }
@@ -48,7 +49,7 @@ void OrcaPlanner::odometryCallback(const nav_msgs::OdometryConstPtr& msg, int ag
   if (!odom_flag_)
     odom_flag_ = true;
 
-  other_odoms_[agent_id] = *msg;
+  other_odoms_[agent_id - 1] = *msg;
 }
 
 bool OrcaPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan)

diff --git a/src/sim_env/config/planner/orca_planner_params.yaml b/src/sim_env/config/planner/orca_planner_params.yaml
@@ -1 +1 @@
-OrcaPlanner:
+# OrcaPlanner:
diff --git a/src/sim_env/launch/app/environment_single.launch.xml b/src/sim_env/launch/app/environment_single.launch.xml
@@ -11,6 +11,8 @@
 -->
 
 <launch>
+    <arg name="agent_number" default="1" />
+    <arg name="agent_id" default="1" />
     <!-- robot's type -->
     <arg name="robot" default="turtlebot3_waffle" />
     <!-- robot's namespace -->
@@ -55,6 +57,8 @@
 
         <!-- MoveBase -->
         <include file="$(find sim_env)/launch/include/navigation/move_base.launch.xml">
+            <arg name="agent_number" value="$(arg agent_number)" />
+            <arg name="agent_id" value="$(arg agent_id)" />
             <arg name="robot_namespace" value="$(arg robot_namespace)" />
             <arg name="start_ns" value="$(arg start_ns)" />
             <arg name="robot" value="$(arg robot)" />

diff --git a/src/sim_env/launch/include/navigation/move_base.launch.xml b/src/sim_env/launch/include/navigation/move_base.launch.xml
@@ -11,6 +11,9 @@
 -->
 
 <launch>
+    <arg name="agent_number" default="1" />
+    <arg name="agent_id" default="1" />
+
     <!-- robot's namespace -->
     <arg name="robot_namespace" />
     <!-- whether use namespace or not -->
@@ -118,9 +121,11 @@
 
         <param name="base_local_planner" value="static_planner/StaticPlanner" if="$(eval arg('local_planner')=='static')" />
 
+        <!-- ORCA -->
         <param name="base_local_planner" value="orca_planner/OrcaPlanner" if="$(eval arg('local_planner')=='orca')" />
+        <param name="OrcaPlanner/agent_number" value="$(arg agent_number)" if="$(eval arg('local_planner')=='orca')" />
+        <param name="OrcaPlanner/agent_id" value="$(arg agent_id)" if="$(eval arg('local_planner')=='orca')" />
         <rosparam file="$(find sim_env)/config/planner/orca_planner_params.yaml" command="load" if="$(eval arg('local_planner')=='orca')" />
-
 
         <!-- loading navigation parameters -->
         <rosparam file="$(eval find('sim_env') + '/config/' + arg('robot') + '/costmap_common_params_' + arg('robot') + '.yaml')" command="load"

diff --git a/src/sim_env/launch/include/robots/start_robots.launch.xml b/src/sim_env/launch/include/robots/start_robots.launch.xml
@@ -1,6 +1,7 @@
 <?xml version='1.0' encoding='utf-8'?>
 <launch>
-	<arg name="robot_number" default="2" />
+	<arg name="agent_number" default="2" />
+	<arg name="agent_id" default="2" />
 	<arg name="robot1_type" value="turtlebot3_waffle" />
 	<arg name="robot1_global_planner" value="lazy" />
 	<arg name="robot1_local_planner" value="orca" />
@@ -16,17 +17,19 @@
 	<arg name="robot2_z_pos" value="0.0" />
 	<arg name="robot2_yaw" value="0.0" />
 	<include file="$(find sim_env)/launch/app/environment_single.launch.xml">
-		<arg name="robot" value="$(eval arg('robot' + str(arg('robot_number')) + '_type'))" />
-		<arg name="global_planner" value="$(eval arg('robot' + str(arg('robot_number')) + '_global_planner'))" />
-		<arg name="local_planner" value="$(eval arg('robot' + str(arg('robot_number')) + '_local_planner'))" />
-		<arg name="robot_namespace" value="robot$(arg robot_number)" />
+		<arg name="agent_number" value="$(arg agent_number)" />
+		<arg name="agent_id" value="$(arg agent_id)" />
+		<arg name="robot" value="$(eval arg('robot' + str(arg('agent_id')) + '_type'))" />
+		<arg name="global_planner" value="$(eval arg('robot' + str(arg('agent_id')) + '_global_planner'))" />
+		<arg name="local_planner" value="$(eval arg('robot' + str(arg('agent_id')) + '_local_planner'))" />
+		<arg name="robot_namespace" value="robot$(arg agent_id)" />
 		<arg name="start_ns" value="true" />
-		<arg name="robot_x" value="$(eval arg('robot' + str(arg('robot_number')) + '_x_pos'))" />
-		<arg name="robot_y" value="$(eval arg('robot' + str(arg('robot_number')) + '_y_pos'))" />
-		<arg name="robot_z" value="$(eval arg('robot' + str(arg('robot_number')) + '_z_pos'))" />
-		<arg name="robot_yaw" value="$(eval arg('robot' + str(arg('robot_number')) + '_yaw'))" />
+		<arg name="robot_x" value="$(eval arg('robot' + str(arg('agent_id')) + '_x_pos'))" />
+		<arg name="robot_y" value="$(eval arg('robot' + str(arg('agent_id')) + '_y_pos'))" />
+		<arg name="robot_z" value="$(eval arg('robot' + str(arg('agent_id')) + '_z_pos'))" />
+		<arg name="robot_yaw" value="$(eval arg('robot' + str(arg('agent_id')) + '_yaw'))" />
 	</include>
-	<include file="$(find sim_env)/launch/include/robots/start_robots.launch.xml" if="$(eval arg('robot_number') &gt; 1)">
-		<arg name="robot_number" value="$(eval arg('robot_number') - 1)" />
+	<include file="$(find sim_env)/launch/include/robots/start_robots.launch.xml" if="$(eval arg('agent_id') &gt; 1)">
+		<arg name="agent_id" value="$(eval arg('agent_id') - 1)" />
 	</include>
 </launch>
diff --git a/src/third_party/dynamic_xml_config/plugins/robot_generate.py b/src/third_party/dynamic_xml_config/plugins/robot_generate.py
@@ -44,9 +44,7 @@ def writeRobotsXml(self, path):
 
         def getRobotArg(name: str, index: int = -1):
             if index == -1:
-                e = RobotGenerator.createElement(
-                    "arg", props={"name": name, "value": "$(eval arg('robot' + str(arg('robot_number')) + '_" + name + "'))"}
-                )
+                e = RobotGenerator.createElement("arg", props={"name": name, "value": "$(eval arg('robot' + str(arg('agent_id')) + '_" + name + "'))"})
             else:
                 e = RobotGenerator.createElement(
                     "arg",
@@ -67,7 +65,8 @@ def getRobotArg(name: str, index: int = -1):
             robots_num = 0
             raise ValueError("There is no robot!")
 
-        launch.append(RobotGenerator.createElement("arg", props={"name": "robot_number", "default": str(robots_num)}))
+        launch.append(RobotGenerator.createElement("arg", props={"name": "agent_number", "default": str(robots_num)}))
+        launch.append(RobotGenerator.createElement("arg", props={"name": "agent_id", "default": str(robots_num)}))
 
         # setting the parameters of robots
         for i in range(robots_num):
@@ -85,37 +84,27 @@ def getRobotArg(name: str, index: int = -1):
 
         # create starting node
         include = RobotGenerator.createElement("include", props={"file": "$(find sim_env)/launch/app/environment_single.launch.xml"})
-        include.append(
-            RobotGenerator.createElement("arg", props={"name": "robot", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_type'))"})
-        )
+        include.append(RobotGenerator.createElement("arg", props={"name": "agent_number", "value": "$(arg agent_number)"}))
+        include.append(RobotGenerator.createElement("arg", props={"name": "agent_id", "value": "$(arg agent_id)"}))
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot", "value": "$(eval arg('robot' + str(arg('agent_id')) + '_type'))"}))
         # planner
         include.append(getRobotArg("global_planner"))
         include.append(getRobotArg("local_planner"))
         # namespace
-        include.append(RobotGenerator.createElement("arg", props={"name": "robot_namespace", "value": "robot$(arg robot_number)"}))
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_namespace", "value": "robot$(arg agent_id)"}))
         if robots_num > 1:
             include.append(RobotGenerator.createElement("arg", props={"name": "start_ns", "value": "true"}))
         else:
             include.append(RobotGenerator.createElement("arg", props={"name": "start_ns", "value": "false"}))
         # pose
-        include.append(
-            RobotGenerator.createElement("arg", props={"name": "robot_x", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_x_pos'))"})
-        )
-        include.append(
-            RobotGenerator.createElement("arg", props={"name": "robot_y", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_y_pos'))"})
-        )
-        include.append(
-            RobotGenerator.createElement("arg", props={"name": "robot_z", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_z_pos'))"})
-        )
-        include.append(
-            RobotGenerator.createElement("arg", props={"name": "robot_yaw", "value": "$(eval arg('robot' + str(arg('robot_number')) + '_yaw'))"})
-        )
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_x", "value": "$(eval arg('robot' + str(arg('agent_id')) + '_x_pos'))"}))
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_y", "value": "$(eval arg('robot' + str(arg('agent_id')) + '_y_pos'))"}))
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_z", "value": "$(eval arg('robot' + str(arg('agent_id')) + '_z_pos'))"}))
+        include.append(RobotGenerator.createElement("arg", props={"name": "robot_yaw", "value": "$(eval arg('robot' + str(arg('agent_id')) + '_yaw'))"}))
 
         # recursive start
-        cycle = RobotGenerator.createElement(
-            "include", props={"file": "$(find sim_env)/launch/include/robots/start_robots.launch.xml", "if": "$(eval arg('robot_number') > 1)"}
-        )
-        cycle.append(RobotGenerator.createElement("arg", props={"name": "robot_number", "value": "$(eval arg('robot_number') - 1)"}))
+        cycle = RobotGenerator.createElement("include", props={"file": "$(find sim_env)/launch/include/robots/start_robots.launch.xml", "if": "$(eval arg('agent_id') > 1)"})
+        cycle.append(RobotGenerator.createElement("arg", props={"name": "agent_id", "value": "$(eval arg('agent_id') - 1)"}))
 
         launch.append(include)
         launch.append(cycle)