Skip to content

feat(rmf_plan_executor): make grid configuration dynamic and configurable via parameters and /map #53

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
arjo129 wants to merge 1 commit into from
Open

feat(rmf_plan_executor): make grid configuration dynamic and configurable via parameters and /map #53

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
157 changes: 150 additions & 7 deletions path_server/rmf_plan_executor/src/lib.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -90,6 +90,54 @@ impl BlockageMonitor {
}
}

#[derive(Clone, Debug, PartialEq)]
pub struct PlanExecutorConfig {
pub grid_width: u32,
pub grid_height: u32,
pub grid_resolution: f32,
pub grid_origin: Pose,
}

impl Default for PlanExecutorConfig {
fn default() -> Self {
let mut origin = Pose::default();
origin.orientation.w = 1.0;
Self {
grid_width: 20,
grid_height: 20,
grid_resolution: 1.0,
grid_origin: origin,
}
}
}

impl PlanExecutorConfig {
pub fn new(grid_width: u32, grid_height: u32, grid_resolution: f32, grid_origin: Pose) -> Self {
Self {
grid_width,
grid_height,
grid_resolution,
grid_origin,
}
}

pub fn with_grid_size(mut self, width: u32, height: u32) -> Self {
self.grid_width = width;
self.grid_height = height;
self
}

pub fn with_resolution(mut self, resolution: f32) -> Self {
self.grid_resolution = resolution;
self
}

pub fn with_origin(mut self, origin: Pose) -> Self {
self.grid_origin = origin;
self
}
}

pub struct PlanExecutor {
pub node: Node,
// TODO(arjoc) The use of BTreeMap here is to make sure robts are ordered by their names
Expand Down Expand Up @@ -134,19 +182,29 @@ fn target_yaw(plan: &Plan, target_idx: usize) -> f32 {

impl PlanExecutor {
pub fn new(node: Node) -> Self {
let mut origin = Pose::default();
origin.orientation.w = 1.0;
Self::with_config(node, PlanExecutorConfig::default())
}

pub fn new_with_config(node: Node, config: PlanExecutorConfig) -> Self {
Self::with_config(node, config)
}

pub fn with_config(node: Node, config: PlanExecutorConfig) -> Self {
let grid = Arc::new(Grid2D::new(
vec![vec![0; config.grid_height as usize]; config.grid_width as usize],
config.grid_resolution,
));
Self {
node,
active_robots: BTreeMap::new(),
plan_release_publishers: HashMap::new(),
safezone_publishers: HashMap::new(),
plan_error_publishers: HashMap::new(),
grid: Arc::new(Grid2D::new(vec![vec![0; 20]; 20], 1.0)),
grid_width: 20,
grid_height: 20,
grid_resolution: 1.0,
grid_origin: origin,
grid,
grid_width: config.grid_width,
grid_height: config.grid_height,
grid_resolution: config.grid_resolution,
grid_origin: config.grid_origin,
latest_map: None,
}
}
Expand Down Expand Up @@ -250,6 +308,26 @@ impl PlanExecutor {
}

pub fn handle_map(&mut self, msg: OccupancyGrid) {
if msg.info.width > 0 && msg.info.height > 0 && msg.info.resolution > 0.0 {
rclrs::log!(
self.node.logger(),
"PlanExecutor reconfiguring grid from map: width={}, height={}, resolution={}, origin=({}, {})",
msg.info.width,
msg.info.height,
msg.info.resolution,
msg.info.origin.position.x,
msg.info.origin.position.y
);
self.grid_width = msg.info.width;
self.grid_height = msg.info.height;
self.grid_resolution = msg.info.resolution;
self.grid_origin = msg.info.origin.clone();
self.grid = Arc::new(Grid2D::new(
vec![vec![0; self.grid_height as usize]; self.grid_width as usize],
self.grid_resolution,
));
}

self.latest_map = Some(msg);
let robot_ids: Vec<_> = self.active_robots.keys().cloned().collect();
for robot_id in robot_ids {
Expand Down Expand Up @@ -933,4 +1011,69 @@ mod tests {
follower.get_semantic_waypoint().trajectory_index
);
}

#[test]
fn test_plan_executor_config_default_and_builders() {
use crate::PlanExecutorConfig;
use ros_env::geometry_msgs::msg::Pose;

let config = PlanExecutorConfig::default();
assert_eq!(config.grid_width, 20);
assert_eq!(config.grid_height, 20);
assert_eq!(config.grid_resolution, 1.0);
assert_eq!(config.grid_origin.orientation.w, 1.0);

let mut custom_origin = Pose::default();
custom_origin.position.x = -10.0;
custom_origin.position.y = -5.0;
custom_origin.orientation.w = 1.0;

let custom_config = PlanExecutorConfig::new(50, 60, 0.5, custom_origin.clone());
assert_eq!(custom_config.grid_width, 50);
assert_eq!(custom_config.grid_height, 60);
assert_eq!(custom_config.grid_resolution, 0.5);
assert_eq!(custom_config.grid_origin.position.x, -10.0);

let builder_config = PlanExecutorConfig::default()
.with_grid_size(100, 200)
.with_resolution(0.2)
.with_origin(custom_origin.clone());
assert_eq!(builder_config.grid_width, 100);
assert_eq!(builder_config.grid_height, 200);
assert_eq!(builder_config.grid_resolution, 0.2);
assert_eq!(builder_config.grid_origin, custom_origin);
}

#[test]
fn test_handle_map() {
use crate::PlanExecutor;
use rclrs::{Context, CreateBasicExecutor};
use ros_env::nav_msgs::msg::OccupancyGrid;

if let Ok(context) = Context::default_from_env() {
let executor = context.create_basic_executor();
if let Ok(node) = executor.create_node("test_handle_map_node") {
let mut plan_executor = PlanExecutor::new(node);
assert_eq!(plan_executor.grid_width, 20);
assert_eq!(plan_executor.grid_height, 20);
assert_eq!(plan_executor.grid_resolution, 1.0);

let mut map_msg = OccupancyGrid::default();
map_msg.info.width = 150;
map_msg.info.height = 200;
map_msg.info.resolution = 0.05;
map_msg.info.origin.position.x = -15.0;
map_msg.info.origin.position.y = -10.0;
map_msg.info.origin.orientation.w = 1.0;

plan_executor.handle_map(map_msg);

assert_eq!(plan_executor.grid_width, 150);
assert_eq!(plan_executor.grid_height, 200);
assert_eq!(plan_executor.grid_resolution, 0.05);
assert_eq!(plan_executor.grid_origin.position.x, -15.0);
assert_eq!(plan_executor.grid_origin.position.y, -10.0);
}
}
}
}
124 changes: 115 additions & 9 deletions path_server/rmf_plan_executor/src/main.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,10 +13,12 @@
// limitations under the License.

use rclrs::{Context, CreateBasicExecutor, IntoPrimitiveOptions, SpinOptions};
use rmf_plan_executor::PlanExecutor;
use rmf_plan_executor::{PlanExecutor, PlanExecutorConfig};
use ros_env::geometry_msgs::msg::Pose;
use ros_env::nav_msgs::msg::{OccupancyGrid, Odometry};
use ros_env::rmf_prototype_msgs::msg::{ParticipantList, Plan};
use std::collections::HashMap;
use std::env;

struct RobotConnections {
_odom_subscription: rclrs::WorkerSubscription<Odometry, PlanExecutor>,
Expand Down Expand Up @@ -44,8 +46,119 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
let mut executor = context.create_basic_executor();
let node = executor.create_node("plan_executor")?;

let param_grid_width = node
.declare_parameter("grid_width")
.default(20i64)
.mandatory()
.map(|p| p.get() as u32)
.unwrap_or(20);

let param_grid_height = node
.declare_parameter("grid_height")
.default(20i64)
.mandatory()
.map(|p| p.get() as u32)
.unwrap_or(20);

let param_grid_resolution = node
.declare_parameter("grid_resolution")
.default(1.0f64)
.mandatory()
.map(|p| p.get() as f32)
.unwrap_or(1.0);

let param_grid_size = node
.declare_parameter("grid_size")
.default(1.0f64)
.mandatory()
.map(|p| p.get() as f32)
.unwrap_or(1.0);

let param_grid_origin_x = node
.declare_parameter("grid_origin_x")
.default(0.0f64)
.mandatory()
.map(|p| p.get())
.unwrap_or(0.0);

let param_grid_origin_y = node
.declare_parameter("grid_origin_y")
.default(0.0f64)
.mandatory()
.map(|p| p.get())
.unwrap_or(0.0);

let args: Vec<String> = env::args().collect();
let get_cli_arg = |flag: &str| -> Option<String> {
args.iter()
.position(|arg| arg == flag)
.and_then(|i| args.get(i + 1).cloned())
.or_else(|| {
let prefix = format!("{}=", flag);
args.iter()
.find_map(|arg| arg.strip_prefix(&prefix).map(String::from))
})
};

let grid_width = get_cli_arg("--grid-width")
.and_then(|s| s.parse::<u32>().ok())
.unwrap_or(param_grid_width);

let grid_height = get_cli_arg("--grid-height")
.and_then(|s| s.parse::<u32>().ok())
.unwrap_or(param_grid_height);

let grid_resolution = get_cli_arg("--grid-resolution")
.or_else(|| get_cli_arg("--grid-size"))
.and_then(|s| s.parse::<f32>().ok())
.unwrap_or_else(|| {
if param_grid_size != 1.0 && param_grid_resolution == 1.0 {
param_grid_size
} else {
param_grid_resolution
}
});

let grid_origin_x = get_cli_arg("--grid-origin-x")
.and_then(|s| s.parse::<f64>().ok())
.unwrap_or(param_grid_origin_x);

let grid_origin_y = get_cli_arg("--grid-origin-y")
.and_then(|s| s.parse::<f64>().ok())
.unwrap_or(param_grid_origin_y);

let mut origin = Pose::default();
origin.position.x = grid_origin_x;
origin.position.y = grid_origin_y;
origin.orientation.w = 1.0;

let config = PlanExecutorConfig {
grid_width,
grid_height,
grid_resolution,
grid_origin: origin,
};

rclrs::log!(
node.logger(),
"PlanExecutor initialized with grid: width={}, height={}, resolution={}, origin=({}, {})",
config.grid_width,
config.grid_height,
config.grid_resolution,
config.grid_origin.position.x,
config.grid_origin.position.y
);

// Create the executor worker
let executor_worker = node.create_worker(PlanExecutor::new(node.clone()));
let executor_worker = node.create_worker(PlanExecutor::with_config(node.clone(), config));

// Subscribe to map to auto-reconfigure grid size, resolution, and origin
let _map_subscription = executor_worker.create_subscription::<OccupancyGrid, _>(
"/map".transient_local().reliable(),
move |executor: &mut PlanExecutor, msg: OccupancyGrid| {
executor.handle_map(msg);
},
)?;

// Create the discovery worker
let discovery_worker = node.create_worker(ExecutorDiscoveryServer::new(
Expand Down Expand Up @@ -73,13 +186,6 @@ fn main() -> Result<(), Box<dyn std::error::Error>> {
},
)?;

let _map_subscription = executor_worker.create_subscription::<OccupancyGrid, _>(
"/map".transient_local().reliable(),
move |executor: &mut PlanExecutor, msg: OccupancyGrid| {
executor.handle_map(msg);
},
)?;

// 2. Subscribe to discovery on the discovery worker to manage odom/plan subscriptions
let _discovery_subscription = rmf_participant_discovery::create_discovery_subscription(
&discovery_worker,
Expand Down
Loading