From 250eb97f69a248dde1d65b8e9a5d89271f9a7613 Mon Sep 17 00:00:00 2001 From: Tim Lehr Date: Thu, 2 Oct 2014 22:59:57 +0200 Subject: [PATCH] add target heading --- buoy.orogen | 3 ++- tasks/ServoingOnWall.cpp | 6 +++++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/buoy.orogen b/buoy.orogen index 143e36b..560743c 100644 --- a/buoy.orogen +++ b/buoy.orogen @@ -393,8 +393,9 @@ task_context "ServoingOnWall" do output_port("aligned_position_cmd", "base::LinearAngular6DCommand") property("distance_to_buoy", "double", 1) - property("angle_to_wall", "double", 0) + property("target_heading", "double", 1.57) property("heading_step_size", "double", 0.1) + property("aligned_distance", "double", 0.5) diff --git a/tasks/ServoingOnWall.cpp b/tasks/ServoingOnWall.cpp index 9e09ab6..43ec1eb 100644 --- a/tasks/ServoingOnWall.cpp +++ b/tasks/ServoingOnWall.cpp @@ -74,6 +74,10 @@ void ServoingOnWall::updateHook() } double heading_step = base::Angle::normalizeRad(base::getYaw(orientation.orientation) + _heading_step_size.get()); + if(heading_step > _target_heading.get()){ + heading_step = _target_heading.get(); + } + Eigen::Vector3d buoy_rel_pos_in_world = orientation.orientation * buoy.world_coord; Eigen::Vector3d buoy_offset = orientation.orientation.inverse() * (Eigen::AngleAxisd(heading_step, Eigen::Vector3d::UnitZ()) * Eigen::Vector3d(_distance_to_buoy.get(),0,0)); @@ -89,7 +93,7 @@ void ServoingOnWall::updateHook() std::cout << "------------------------------------------------------" << std::endl; std::cout << "result: + " << aligned_cmd.linear.transpose() << std::endl; - if(world_cmd.linear.norm() < _aligned_distance.get() && state() != ALIGNED){ + if(world_cmd.linear.norm() < _aligned_distance.get() && base::getYaw(orientation.orientation) && state() != ALIGNED){ state(ALIGNED); }