Docking correctness with camera team

[JustinShih0918]

Problem

• while doing docking, we struggle to the issue that we can not pick up the item correctly.
• this may cause by several reason
  • move forward distance not enough
    • this will be fix by Main
  • does not align the item
    • we are going to fix this problem

About docking

everything about docking is in opennav_docking

it has its own BT, spinning Engine, self-define message type, controller, server
To be specific, docking behavior of our project is mainly determine by the controller & server. (but this is because we don't need another functionality in original opennav_docking)

Docking Server

Docking Server define the behavior of the lifecycle of docking, we can find function like on_activate, on_configure, etc.

the core function is dockRobot().

While in dockRobot(), we take the following action.

1. get everything we need to act a dock
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/docking_server.cpp

   Lines 214 to 215 in a0700cb

   	auto goal = docking_action_server_->get_current_goal();
   	auto result = std::make_shared<DockRobot::Result>();

2. sending the robot to its staging point via navigator
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/docking_server.cpp

   Lines 248 to 260 in a0700cb

   	publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE);
   	const auto initial_staging_pose = dock->getStagingPose();
   	const auto robot_pose = getRobotPoseInFrame(
   	initial_staging_pose.header.frame_id);
   	if (!goal->navigate_to_staging_pose ||
   	utils::l2Norm(robot_pose.pose, initial_staging_pose.pose) < dock_prestaging_tolerance_)
   	{
   	RCLCPP_INFO(get_logger(), "\033[1;32m Robot already within pre-staging pose tolerance for dock \033[0m");
   	} else {
   	navigator_->goToPose(
   	initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time));
   	// RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");
   	}

3. perform dock from staging point
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/docking_server.cpp

   Lines 248 to 260 in a0700cb

   	publishDockingFeedback(DockRobot::Feedback::NAV_TO_STAGING_POSE);
   	const auto initial_staging_pose = dock->getStagingPose();
   	const auto robot_pose = getRobotPoseInFrame(
   	initial_staging_pose.header.frame_id);
   	if (!goal->navigate_to_staging_pose ||
   	utils::l2Norm(robot_pose.pose, initial_staging_pose.pose) < dock_prestaging_tolerance_)
   	{
   	RCLCPP_INFO(get_logger(), "\033[1;32m Robot already within pre-staging pose tolerance for dock \033[0m");
   	} else {
   	navigator_->goToPose(
   	initial_staging_pose, rclcpp::Duration::from_seconds(goal->max_staging_time));
   	// RCLCPP_INFO(get_logger(), "Successful navigation to staging pose");
   	}

4. the actual perform function is approachDock(dock, dock_pose) at line 277
   this function will first check if we need to stop docking, compute the Twist command and send via controller, then check if we need to stop because of the obstacle(controller_->computeIfNeedStop(dock_pose.pose)) via we perform docking.
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/docking_server.cpp

   Lines 398 to 460 in a0700cb

   	bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped & dock_pose)
   	{
   	rclcpp::Rate loop_rate(controller_frequency_);
   	auto start = this->now();
   	auto timeout = rclcpp::Duration::from_seconds(dock_approach_timeout_);
   	while (rclcpp::ok()) {
   	publishDockingFeedback(DockRobot::Feedback::CONTROLLING);
   	// Determine if we want to stop
   	if(stop_robot_) {
   	publishZeroVelocity();
   	throw opennav_docking_core::StopRobot(
   	"StopRobot activate, stop the robot");
   	}
   	// Stop and report success if connected to dock
   	if (dock->plugin->isDocked() || dock->plugin->isCharging()) {
   	publishZeroVelocity();
   	return true;
   	}
   	// Stop if cancelled/preempted
   	if (checkAndWarnIfCancelled(docking_action_server_, "dock_robot") ||
   	checkAndWarnIfPreempted(docking_action_server_, "dock_robot"))
   	{
   	return false;
   	}
   	// Update perception
   	if (!dock->plugin->getRefinedPose(dock_pose)) {
   	throw opennav_docking_core::FailedToDetectDock("Failed dock detection");
   	}
   	// ! Transform target_pose into base_link frame
   	geometry_msgs::msg::PoseStamped target_pose = dock_pose;
   	target_pose.header.stamp = rclcpp::Time(0);
   	tf2_buffer_->transform(target_pose, target_pose, base_frame_);
   	// Compute and publish controls
   	geometry_msgs::msg::Twist command;
   	if (!controller_->computeVelocityCommand(target_pose.pose, command, dock_backwards_)) {
   	throw opennav_docking_core::FailedToControl("Failed to get control");
   	}
   	if (this->now() - start > timeout) {
   	throw opennav_docking_core::FailedToControl(
   	"Timed out approaching dock");
   	}
   	// Compute if rival is on the way
   	if (controller_->computeIfNeedStop(dock_pose.pose)) {
   	publishZeroVelocity();
   	throw opennav_docking_core::BlockByRival(
   	"Goal blocked by rival, stop the robot");
   	}
   	vel_publisher_->publish(command);
   	loop_rate.sleep();
   	}
   	return false;
   	}

Controller

Controller define the util function of actually control of the robot.
trace by yourself.

how to perform docking

the command rule is describe as follow

ros2 action send_goal /dock_robot opennav_docking_msgs/action/DockRobot "{
  dock_type: 'your_dock_type',
  use_dock_id: false,
  navigate_to_staging_pose: true, // do we need to navigate to staging point
  max_staging_time: 1000.0, // time for navigate to staging point
  dock_pose: {
    header: {
      frame_id: 'map'
    },
    pose: {
      position: {x: 1.0, y: 2.0, z: 0.0}, // x,y stand for staging point position, z stand for offset 
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    }
  }
}"

the dock_type support

default and recommend to use

• mission_dock_x
• mission_dock_y

Category	Keywords	Effect
Controller	fast, slow	Adjusts speed profiles.
	linearBoost	Likely increases max linear acceleration/velocity.
	angularBoost	Likely increases max rotational acceleration/velocity.
Goal Checker	precise	Strict tolerance for success.
	loose	Relaxed tolerance (good for "Rush" mode).
Docking Style	ordinary	Standard behavior.
	gentle	Soft deceleration/approach.
	rush	Aggressive/competition mode.

can be append in any order, for example:

You can mix and match these strings. The underscores _ are the standard separator.

• Safe / Testing Modes:
• dock_x_ordinary_slow_precise
• dock_y_gentle_slow
• Competition / Speed Modes:
• dock_x_rush_linearBoost_loose
• dock_y_fast_angularBoost

Issue

To increase the chance to get object we want with Aru Code on it, we need to cooperate with vision team.

• Given by vision team
  • dx, dy of object respect to robot pose
• We need to
  • adjust the direction slightly respect to the given dx, dy

To be specific:

1. add util functions in controller.cpp, subscribe vision topic, get dx,dy, robot_pose as input, adjust x or y according to dock_type. e.g. if mission_dock_x, you use dx to publish /cmd_vel with linear_x is some value and other value is 0.
2. in docking_server.cpp, before main control loop, given a parameter second for this feature to try, publish the /cmd_vel.

[chuangtsh]

default and recommend to use

• mission_dock_x
• mission_dock_y

@JustinShih0918 What does this mean? I couldn't recognize these two.

Additionally, how do I access dock_type in controller.cpp? As far as I know, 'docking_server.cpp' has the server to that entier action. Does it mean I should write a publisher in docking_server and a subscriber in controller?

Also for clarification, one main part of this issue for me is to come up with a way to map dx and dy to /cmd_vel (possibly refering to dock_type and some other math), and test it with camera team?

[JustinShih0918]

@chuangtsh

dock type

ros2 action send_goal /dock_robot opennav_docking_msgs/action/DockRobot "{
  dock_type: 'your_dock_type',
  use_dock_id: false,
  navigate_to_staging_pose: true, // do we need to navigate to staging point
  max_staging_time: 1000.0, // time for navigate to staging point
  dock_pose: {
    header: {
      frame_id: 'map'
    },
    pose: {
      position: {x: 1.0, y: 2.0, z: 0.0}, // x,y stand for staging point position, z stand for offset 
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    }
  }
}"

the "dock_type" is refer to the part 'your_dock_type', different dock_type can result in different behave of docking action.
For example, "mission_dock_x", means normal mission mode (no fast rotating, no speedup, etc.), and the offset should be add on x-axis, that is, if the command is as follow:

ros2 action send_goal /dock_robot opennav_docking_msgs/action/DockRobot "{
  dock_type: 'mission_dock_x',
  use_dock_id: false,
  navigate_to_staging_pose: true, // do we need to navigate to staging point
  max_staging_time: 1000.0, // time for navigate to staging point
  dock_pose: {
    header: {
      frame_id: 'map'
    },
    pose: {
      position: {x: 1.0, y: 2.0, z: 1.0}, // x,y stand for staging point position, z stand for offset 
      orientation: {x: 0.0, y: 0.0, z: 0.0, w: 1.0}
    }
  }
}"

the staging point is (1,2), the actually docking point is (1+1,2) = (2,2)

how to access dock_type

Two things you have to know before keep reading

1. the dock_type is sent by an action, and the nav2_param.yaml file have defined what param value should a particular dock_type use
   

   Eurobot-2026-Navigation2/src/navigation2_run/params/nav2_params_default.yaml

   Lines 356 to 429 in a0700cb

   	docking_server:
   	ros__parameters:
   	controller_frequency: 20.0
   	initial_perception_timeout: 5.0
   	wait_charge_timeout: 5.0
   	dock_approach_timeout: 3.0
   	max_retries: 0
   	base_frame: "base_footprint"
   	fixed_frame: "map"
   	dock_backwards: false
   	dock_prestaging_tolerance: 0.01
   	velocity_smoother_wait: 0.12
   	# Types of docks
   	dock_plugins: ["dock"]
   	dock:
   	plugin: 'opennav_docking::SimpleChargingDock'
   	docking_threshold: 0.005
   	use_debounce: true
   	xy_debounce_threshold: 2
   	yaw_debounce_threshold: 2
   	use_dynamic_offset: true
   	staging_x_offset: 0.35
   	staging_y_offset: 0.35
   	staging_yaw_offset: 1.57
   	use_battery_status: false
   	use_stall_detection: false
   	base_frame: "base_footprint"
   	docks: ['home_dock']
   	home_dock:
   	type: 'dock'
   	frame: map
   	pose: [0.0, 0.0, 0.0]
   	# dock_database: /my/path/to/dock_database.yaml
   	controllers: ["Ordinary", "Gentle", "Rush"]
   	Ordinary:
   	max_linear_vel: 0.3
   	min_linear_vel: 0.0
   	linear_kp_accel_vel: 0.5
   	linear_ki_accel_vel: 0.1
   	linear_kp_decel_dis: 3.0
   	angular_kp: 4.0
   	deceleration_distance: 0.05
   	reserved_distance: 0.0
   	stop_degree: 120.0
   	rival_radius: 0.44
   	Gentle:
   	max_linear_vel: 0.3
   	min_linear_vel: 0.0
   	linear_kp_accel_vel: 0.5
   	linear_ki_accel_vel: 0.1
   	linear_kp_decel_dis: 3.0
   	angular_kp: 4.0
   	deceleration_distance: 0.1
   	reserved_distance: 0.0
   	stop_degree: 120.0
   	rival_radius: 0.44
   	Rush:
   	max_linear_vel: 0.3
   	min_linear_vel: 0.0
   	linear_kp_accel_vel: 0.5
   	linear_ki_accel_vel: 0.1
   	linear_kp_decel_dis: 3.0
   	angular_kp: 4.0
   	deceleration_distance: 0.03
   	reserved_distance: 0.0
   	stop_degree: 120.0
   	rival_radius: 0.44

2. we are using SimpleChargingDock as our docking mode. This is also defined in nav2_param.yaml
   

   Eurobot-2026-Navigation2/src/navigation2_run/params/nav2_params_default.yaml

   Lines 369 to 383 in a0700cb

   	# Types of docks
   	dock_plugins: ["dock"]
   	dock:
   	plugin: 'opennav_docking::SimpleChargingDock'
   	docking_threshold: 0.005
   	use_debounce: true
   	xy_debounce_threshold: 2
   	yaw_debounce_threshold: 2
   	use_dynamic_offset: true
   	staging_x_offset: 0.35
   	staging_y_offset: 0.35
   	staging_yaw_offset: 1.57
   	use_battery_status: false
   	use_stall_detection: false
   	base_frame: "base_footprint"

the data flow of dock_type

1. user(or the MAIN Team) type in the dock_type action command
2. the docking action tick, use input port to get dock_type
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking_bt/src/dock_robot.cpp

   Lines 31 to 42 in a0700cb

   	void DockRobotAction::on_tick()
   	{
   	// Get core inputs about what to perform
   	if (getInput("use_dock_id", goal_.use_dock_id)) {
   	getInput("dock_id", goal_.dock_id);
   	} else {
   	getInput("dock_pose", goal_.dock_pose);
   	getInput("dock_type", goal_.dock_type);
   	getInput("max_staging_time", goal_.max_staging_time);
   	}
   	getInput("navigate_to_staging_pose", goal_.navigate_to_staging_pose);
   	}

3. the docking plugin SimpleChargingDock get the dock_type, pass the nav_type_selector
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/simple_charging_dock.cpp

   Line 188 in a0700cb

   nav_type_selector_->setType(dock_type, offset_direction_, staging_pose, pose.position.z);

4. the nav_type_selector parse the incoming command, determine the dock_type string data, as well as the offset direction (add offset on x or y, check more).
5. publisher in nav_type_selector publish the parsed dock_type to the topic /dock_controller_type
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/nav_type_selector.cpp

   Line 10 in a0700cb

   dock_controller_selector_pub_ = node_->create_publisher<std_msgs::msg::String>("/dock_controller_type", rclcpp::QoS(10).reliable().transient_local());

6. the controller.cpp subscribe on the topic, store into param_name_
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/controller.cpp

   Lines 80 to 86 in a0700cb

   	dock_controller_selector_sub_ = node->create_subscription<std_msgs::msg::String>(
   	"/dock_controller_type",
   	rclcpp::QoS(10).reliable().transient_local(),
   	[this](const std_msgs::msg::String::SharedPtr msg) {
   	param_name_ = msg->data;
   	updateParams();
   	});

7. if the callback of the subscription is ticked, we have to update the param we use in controller.
   

   Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/controller.cpp

   Lines 302 to 319 in a0700cb

   	void Controller::updateParams() {
   	node_->get_parameter(param_name_ + ".max_linear_vel", max_linear_vel_);
   	node_->get_parameter(param_name_ + ".min_linear_vel", min_linear_vel_);
   	node_->get_parameter(param_name_ + ".max_angular_vel", max_angular_vel_);
   	node_->get_parameter(param_name_ + ".min_angular_vel", min_angular_vel_);
   	node_->get_parameter(param_name_ + ".max_linear_acc", max_linear_acc_);
   	node_->get_parameter(param_name_ + ".max_angular_acc", max_angular_acc_);
   	node_->get_parameter(param_name_ + ".linear_ki_accel_vel", linear_ki_accel_vel_);
   	node_->get_parameter(param_name_ + ".linear_kp_accel_vel", linear_kp_accel_vel_);
   	node_->get_parameter(param_name_ + ".linear_kp_decel_dis", linear_kp_decel_dis_);
   	node_->get_parameter(param_name_ + ".linear_kp_decel_vel", linear_kp_decel_vel_);
   	node_->get_parameter(param_name_ + ".angular_kp", angular_kp_);
   	node_->get_parameter(param_name_ + ".look_ahead_distance", look_ahead_distance_);
   	node_->get_parameter(param_name_ + ".deceleration_distance", deceleration_distance_);
   	node_->get_parameter(param_name_ + ".reserved_distance", reserved_distance_);
   	node_->get_parameter(param_name_ + ".stop_degree", stop_degree_);
   	node_->get_parameter(param_name_ + ".rival_radius", rival_radius_);
   	}

as the result, you can get the parsed dock_type by the variable param_name_ in controller.cpp, and the value of param_name_ will be ordinary, gentle, rush. you can checkout for nav_type_selector for more parsing detail.

how to add a new dock type

I think it is good to use dock_type to separate the new feature and the old function, moreover, we will not loss the old feature if we fail on the new one

1. add a new dock type in param.yaml
   

   Eurobot-2026-Navigation2/src/navigation2_run/params/nav2_params_default.yaml

   Line 393 in a0700cb

   controllers: ["Ordinary", "Gentle", "Rush"]

2. define the param detail like other dock type
3. add the parsing rule in nav_type_selector
4. do what you want in controller.cpp

the core requirement of the issue

the Camera Team will give you dx,dy (it will be a topic), for example, for a (dx,dy) = (1,1), and your current robot pose is at (2,1)
this means the actually pose you have to go is (3,2), then we should move the robot to (3,2). And how can you move to (3,2)? use the controller.cpp.

For more implement detail, you can discuss with me and @yuhsiang1117.
@yuhsiang1117 please help us with this issue if @chuangtsh have anything not clear, you can discuss directly.

[chuangtsh]

@JustinShih0918 @yuhsiang1117

Eurobot-2026-Navigation2/src/opennav_docking/opennav_docking/src/docking_server.cpp

Lines 262 to 277 in a0700cb

	// Construct initial estimate of where the dock is located in fixed_frame
	auto dock_pose = utils::getDockPoseStamped(dock, rclcpp::Time(0));
	tf2_buffer_->transform(dock_pose, dock_pose, fixed_frame_);
	// Get initial detection of dock before proceeding to move
	doInitialPerception(dock, dock_pose);
	// RCLCPP_INFO(get_logger(), "Successful initial dock detection");
	// ** Docking control loop: while not docked, run controller
	rclcpp::Time dock_contact_time;
	controller_->velocityInit(dock_pose.pose); // ** Set total distance for velocity control
	RCLCPP_INFO(get_logger(), "\033[1;90m Starting docking control loop. \033[0m");
	while (rclcpp::ok()) {
	try {
	// Approach the dock using control law
	if (approachDock(dock, dock_pose)) {

Why don't we just substitute the content of dock_pose with (dx, dy) from the Camera Team in docking_server.cpp, possibly in the above part? The idea arises when I was examining the controller logic, which I believe many part in controller.cpp should be reused for docking with camera.

If I directly subscribe to (dx, dy) in controller.cpp and calculate /cmd_vel (possibly with my to-be-created PI control or other stuff), then it would probably be another well-functioned controller. Or if I don't write another well-functioned controller, then I think it makes more sense to just substitute the source of dock_pose, and simply let the original action server and controller finisher the job.