[Ninja][Navigation] Implimentation of velocity control interface with…

… joystick.
sugihara-16 · Jan 17, 2025 · a6e5fc7 · a6e5fc7
1 parent 3de0a99
commit a6e5fc7
Show file tree

Hide file tree

Showing 6 changed files with 96 additions and 7 deletions.
diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h
@@ -259,6 +259,7 @@ namespace aerial_robot_navigation
     bool param_verbose_;
 
     bool joy_rotation_flag_;
+    bool joy_duplicated_flag_;
 
     uint8_t navi_state_;
 

diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp
@@ -460,6 +460,8 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
 
   teleop_reset_time_ = teleop_reset_duration_ + ros::Time::now().toSec();
 
+  if(joy_duplicated_flag_) return;
+
   double raw_x_cmd = 0;
   double raw_y_cmd = 0;
   double raw_z_cmd = joy_cmd.axes[PS3_AXIS_STICK_RIGHT_UPWARDS];

diff --git a/robots/beetle/include/beetle/beetle_navigation.h b/robots/beetle/include/beetle/beetle_navigation.h
@@ -117,11 +117,11 @@ namespace aerial_robot_navigation
     void rosParamInit() override;
     virtual void convertTargetPosFromCoG2CoM();
     virtual void assemblyNavCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
+    virtual void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) override; 
 
   private:
     void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg) override;
     void setAssemblyFinalTargetBaselinkRotCallback(const spinal::DesireCoordConstPtr & msg);
-    void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) override; 
     void rotateContactPointFrame();
     boost::shared_ptr<BeetleRobotModel> beetle_robot_model_;
     map<string, ros::Subscriber> assembly_flag_subs_;

diff --git a/robots/beetle/src/beetle_navigation.cpp b/robots/beetle/src/beetle_navigation.cpp
@@ -46,6 +46,13 @@ void BeetleNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
 
 void BeetleNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
 {
+  auto copied_joy_msg = boost::make_shared<sensor_msgs::Joy>(*joy_msg);  
+
+  if(joy_duplicated_flag_)
+    {
+      BaseNavigator::joyStickControl(copied_joy_msg);
+      return;
+    }
   sensor_msgs::Joy joy_cmd;
   if(joy_msg->axes.size() == PS3_AXES && joy_msg->buttons.size() == PS3_BUTTONS)
     {
@@ -139,7 +146,9 @@ void BeetleNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
       joy_pitch_positive_flag_ = false;
       joy_pitch_negative_flag_ = false;
     }
-  BaseNavigator::joyStickControl(joy_msg);
+
+
+  BaseNavigator::joyStickControl(copied_joy_msg);
 }
 
 void BeetleNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg)

diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -61,8 +61,16 @@ namespace aerial_robot_navigation
     void setTargetCoMPoseFromCurrState();
     void setTargetJointPosFromCurrState();
     void setCoM2Base(const KDL::Frame com2base){com2base_ = com2base;}
-    inline void setTargetVelCandX( float value){  target_vel_candidate_.setX(value);}
-    inline void setTargetVelCandY( float value){  target_vel_candidate_.setY(value);}
+    inline void setTargetVelCandX( float value)
+    {
+      std::lock_guard<std::mutex> lock(mutex_cand_vel_);
+      target_vel_candidate_.setX(value);
+    }
+    inline void setTargetVelCandY( float value)
+    {
+      std::lock_guard<std::mutex> lock(mutex_cand_vel_);
+      target_vel_candidate_.setY(value);
+    }
     inline void setTargetVelCandZ( float value){  target_vel_candidate_.setZ(value);}
     inline void setTargetVelCand( tf::Vector3 value){  target_vel_candidate_ = value ;}
     inline void setFinalTargetPosCandX( float value){  target_final_pos_candidate_.setX(value);}
@@ -88,6 +96,7 @@ namespace aerial_robot_navigation
     void updateAssemblyTree();
 
     void assemblyNavCallback(const aerial_robot_msgs::FlightNavConstPtr & msg) override;
+    void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) override;
 
   private:
     void convertTargetPosFromCoG2CoM() override;
@@ -143,6 +152,8 @@ namespace aerial_robot_navigation
     double com_yaw_change_thresh_;
 
     bool disassembly_flag_;
+
+    std::mutex mutex_cand_vel_;
 
   };
   template<> inline KDL::Frame NinjaNavigator::getCom2Base()

diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
@@ -468,7 +468,7 @@ void NinjaNavigator::convertTargetPosFromCoG2CoM()
   if( getNaviState() == HOVER_STATE ||
       getNaviState() == TAKEOFF_STATE){
     setTargetPos(target_pos);
-    setTargetVel(target_vel);
+    // setTargetVel(target_vel);
     setTargetYaw(target_rot.z());
     forceSetTargetBaselinkRot(target_rot);  
   }
@@ -700,7 +700,7 @@ 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);
-
+          
           //update joint info for FK
           ModuleData& data = assembled_modules_data_[id];
           int axis;
@@ -709,7 +709,7 @@ 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:
@@ -930,6 +930,72 @@ void NinjaNavigator::assemblyNavCallback(const aerial_robot_msgs::FlightNavConst
     }
 }
 
+void NinjaNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
+{
+  if(getCurrentAssembled())
+    joy_duplicated_flag_ = true;
+  else
+    joy_duplicated_flag_ = false;
+
+  auto copied_joy_msg = boost::make_shared<sensor_msgs::Joy>(*joy_msg);  
+
+  sensor_msgs::Joy joy_cmd;
+  if(joy_msg->axes.size() == PS3_AXES && joy_msg->buttons.size() == PS3_BUTTONS)
+    {
+      joy_cmd = (*joy_msg);
+    }
+  else if(joy_msg->axes.size() == PS4_AXES && joy_msg->buttons.size() == PS4_BUTTONS)
+    {
+      joy_cmd = ps4joyToPs3joyConvert(*joy_msg);
+    }
+  else
+    {
+      ROS_WARN("the joystick type is not supported (buttons: %d, axes: %d)", (int)joy_msg->buttons.size(), (int)joy_msg->axes.size());
+      return;
+    }
+
+  /* Translational Contol*/
+
+  double raw_x_cmd = joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS];
+  double raw_y_cmd = joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS];
+  double raw_z_cmd = joy_cmd.axes[PS3_AXIS_STICK_RIGHT_UPWARDS];
+  double raw_yaw_cmd = joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS];
+
+  if(fabs(raw_x_cmd) >= joy_stick_deadzone_ || fabs(raw_y_cmd) >= joy_stick_deadzone_)
+    {
+      asm_teleop_reset_time_ = asm_teleop_reset_duration_ + ros::Time::now().toSec();
+      switch (asm_xy_control_mode_)
+        {
+        case POS_CONTROL_MODE:
+          {
+            if(fabs(raw_x_cmd) < joy_stick_deadzone_) raw_x_cmd = 0;
+            if(fabs(raw_y_cmd) < joy_stick_deadzone_) raw_y_cmd = 0;
+
+            /* vel command */
+            setTargetVelCandX(raw_x_cmd * max_teleop_xy_vel_);
+            setTargetVelCandY(raw_y_cmd * max_teleop_xy_vel_);
+            asm_xy_control_mode_ = VEL_CONTROL_MODE;
+            break;
+          }
+        case VEL_CONTROL_MODE:
+          {
+            if(fabs(raw_x_cmd) < joy_stick_deadzone_) raw_x_cmd = 0;
+            if(fabs(raw_y_cmd) < joy_stick_deadzone_) raw_y_cmd = 0;
+
+            /* vel command */
+            setTargetVelCandX(raw_x_cmd * max_teleop_xy_vel_);
+            setTargetVelCandY(raw_y_cmd * max_teleop_xy_vel_);
+            break;
+          }
+        default:
+          {
+            break;
+          }
+        }
+    }
+  BeetleNavigator::joyStickControl(copied_joy_msg);
+}
+
 
 void NinjaNavigator::morphingProcess()
 {