[Ninja][Navigation] Enable to send target morhping velocity.

sugihara-16 · Jan 17, 2025 · 124093e · 124093e
1 parent a6e5fc7
commit 124093e
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 6 deletions.
diff --git a/robots/ninja/config/NavigationConfig.yaml b/robots/ninja/config/NavigationConfig.yaml
@@ -42,7 +42,7 @@ navigation:
   com_yaw_change_thresh: 0.02
 
   #morphing
-  morphing_vel: 0.03
+  default_morphing_vel: 0.04
   joint_process_func: 2 # 0 -> CONSTANT, 1 -> FRAC, 2 -> EXP
   free_joint_flag: false
 

diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -137,7 +137,7 @@ namespace aerial_robot_navigation
     KDL::Frame test_frame_;
 
     int module_joint_num_;
-    double morphing_vel_;
+    double default_morphing_vel_;
     double joint_pos_chnage_thresh_;
     int joint_process_func_;
 

diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
@@ -660,7 +660,7 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst
     {
       for(int i = 0; i < msg->name.size(); i++)
         {
-          if(msg->position.size() !=  msg->name.size())
+          if(msg->position.size() !=  msg->name.size()) 
             {
               ROS_ERROR("The joint position num and name num are different in ros msgs [%d vs %d]",
                         (int)msg->position.size(), (int)msg->name.size());
@@ -700,7 +700,13 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst
           //add to joint control msg
           std::string target_joint_name = joint_map[joint_name];
           double target_joint_angle = msg->position.at(i);
-
+
+          //setup morphing velocity
+          double morphing_vel;
+          if(msg->velocity.empty())
+            morphing_vel = default_morphing_vel_;
+          else
+            morphing_vel = msg->velocity.at(0);
           //update joint info for FK
           ModuleData& data = assembled_modules_data_[id];
           int axis;
@@ -709,7 +715,8 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst
           data.goal_joint_pos_(axis) = target_joint_angle;
           data.start_joint_pos_(axis) = data.des_joint_pos_(axis);
           data.first_joint_processed_time_[axis] = ros::Time::now().toSec();
-          double conv_time = abs(data.goal_joint_pos_(axis) - data.des_joint_pos_(axis)) / morphing_vel_;
+          double conv_time = abs(data.goal_joint_pos_(axis) - data.des_joint_pos_(axis)) / morphing_vel;
+
           switch(joint_process_func_)
             {
             case CONSTANT:
@@ -1120,7 +1127,7 @@ void NinjaNavigator::morphingProcess()
 void NinjaNavigator::rosParamInit()
 {
   ros::NodeHandle nh(nh_, "navigation");
-  getParam<double>(nh, "morphing_vel", morphing_vel_, M_PI/4.0);
+  getParam<double>(nh, "default_morphing_vel", default_morphing_vel_, M_PI/4.0);
   getParam<double>(nh, "joint_pos_change_thresh", joint_pos_chnage_thresh_, 0.01);
   getParam<int>(nh, "joint_process_func", joint_process_func_, CONSTANT);
   getParam<bool>(nh, "free_joint_flag", free_joint_flag_, false);