[Ninja][Navigation][WIP] Add some variables to treat navigation mode …

…in assembled state.
sugihara-16 · Dec 20, 2024 · bf59518 · bf59518
1 parent 6924593
commit bf59518
Show file tree

Hide file tree

Showing 2 changed files with 4 additions and 1 deletion.
diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -94,6 +94,9 @@ namespace aerial_robot_navigation
     map<string, ros::Subscriber> module_joints_subs_;
 
     tf::Vector3 target_vel_candidate_;
+    double asm_vel_nav_threshold_;
+    int asm_xy_control_mode_;
+    int asm_vel_based_waypoint_;
 
     std::map<int, ModuleData> assembled_modules_data_;
     std::vector<double> joint_pos_errs_;

diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
@@ -707,7 +707,7 @@ void NinjaNavigator::assemblyNavCallback(const aerial_robot_msgs::FlightNavConst
         tf::Vector3 target_delta = getTargetPosCand() - target_cog_pos;
         target_delta.setZ(0);
 
-        if(target_delta.length() > vel_nav_threshold_)
+        if(target_delta.length() > asm_vel_nav_threshold_)
           {
             ROS_WARN("start vel nav control for waypoint");
             vel_based_waypoint_ = true;