add target heading

auv-avalon · Oct 2, 2014 · 250eb97 · 250eb97
1 parent 7a2b75e
commit 250eb97
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git 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
@@ -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);
         }