Skip to content

Commit

Permalink
CI fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
Jakubach committed Nov 13, 2024
1 parent 569a2b3 commit b60d67a
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 5 deletions.
8 changes: 4 additions & 4 deletions nav2_docking/opennav_docking/src/controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -60,10 +60,10 @@ Controller::Controller(const rclcpp_lifecycle::LifecycleNode::SharedPtr & node)
std::bind(&Controller::dynamicParametersCallback, this, std::placeholders::_1));
}

bool Controller::computeVelocityCommand(
const geometry_msgs::msg::Pose & pose,
const geometry_msgs::msg::Pose & robot_pose,
geometry_msgs::msg::Twist & cmd, bool backward)
bool Controller::computeVelocityCommand(
const geometry_msgs::msg::Pose & pose,
const geometry_msgs::msg::Pose & robot_pose,
geometry_msgs::msg::Twist & cmd, bool backward)
{
std::lock_guard<std::mutex> lock(dynamic_params_lock_);
cmd = control_law_->calculateRegularVelocity(pose, robot_pose, backward);
Expand Down
3 changes: 2 additions & 1 deletion nav2_docking/opennav_docking/src/docking_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -481,7 +481,8 @@ bool DockingServer::approachDock(Dock * dock, geometry_msgs::msg::PoseStamped &
command->header.stamp = now();
geometry_msgs::msg::PoseStamped robot_pose = getRobotPoseInFrame(dock_pose.header.frame_id);
if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose,
command->twist, dock_backwards_)) {
command->twist, dock_backwards_))
{
throw opennav_docking_core::FailedToControl("Failed to get control");
}
vel_publisher_->publish(std::move(command));
Expand Down

0 comments on commit b60d67a

Please sign in to comment.