[Ninja][Navigation] Workaroud to set the initial shape of assembled s…

…tructure according to each joint angles.
sugihara-16 · Nov 19, 2024 · 1841abf · 1841abf
1 parent ab16840
commit 1841abf
Show file tree

Hide file tree

Showing 2 changed files with 78 additions and 0 deletions.
diff --git a/robots/ninja/include/ninja/ninja_navigation.h b/robots/ninja/include/ninja/ninja_navigation.h
@@ -53,6 +53,7 @@ namespace aerial_robot_navigation
     void setTargetComRot(KDL::Rotation target_com_rot){ target_com_rot_ = target_com_rot;}
     void setGoalComRot(KDL::Rotation goal_com_rot){ goal_com_rot_ = goal_com_rot;}
     void setTargetCoMPoseFromCurrState();
+    void setTargetJointPosFromCurrState();
     void setCoM2Base(const KDL::Frame com2base){com2base_ = com2base;}
     void morphingProcess();
 
@@ -71,16 +72,22 @@ namespace aerial_robot_navigation
     void convertTargetPosFromCoG2CoM() override;
     void setGoalCoMRotCallback(const spinal::DesireCoordConstPtr & msg);
     void assemblyJointPosCallback(const sensor_msgs::JointStateConstPtr& msg);
+    void jointStateCallback(const sensor_msgs::JointStateConstPtr& state);
+    void moduleJointsCallback(const sensor_msgs::JointStateConstPtr& state);
     void comRotationProcess();
 
     ros::Publisher target_com_pose_pub_;
     boost::shared_ptr<NinjaRobotModel> ninja_robot_model_;
     ros::Subscriber target_com_rot_sub_;
     ros::Subscriber target_joints_pos_sub_;
+    ros::Subscriber joint_state_sub_;
     ros::Publisher joint_control_pub_;
+    ros::Publisher dock_joints_pos_pub_;
+    map<string, ros::Subscriber> module_joints_subs_;
 
     std::map<int, ModuleData> assembled_modules_data_;
     std::vector<double> joint_pos_errs_;
+    std::map<int, KDL::JntArray> all_modules_joints_pos_;
 
     KDL::Rotation goal_com_rot_;
     KDL::Rotation target_com_rot_;

diff --git a/robots/ninja/src/ninja_navigation.cpp b/robots/ninja/src/ninja_navigation.cpp
@@ -21,16 +21,25 @@ void NinjaNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
   ninja_robot_model_ = boost::dynamic_pointer_cast<NinjaRobotModel>(robot_model);
   target_com_pose_pub_ = nh_.advertise<geometry_msgs::Pose>("target_com_pose", 1); //for debug
   joint_control_pub_ = nh_.advertise<sensor_msgs::JointState>("joints_ctrl", 1);
+  dock_joints_pos_pub_ = nh_.advertise<sensor_msgs::JointState>("dock_joints_pos", 1);
   target_com_rot_sub_ = nh_.subscribe("/target_com_rot", 1, &NinjaNavigator::setGoalCoMRotCallback, this);
   target_joints_pos_sub_ = nh_.subscribe("/assembly/target_joint_pos", 1, &NinjaNavigator::assemblyJointPosCallback, this);
+  joint_state_sub_ = nh_.subscribe("joint_states", 1, &NinjaNavigator::jointStateCallback, this);
   tfBuffer_.setUsingDedicatedThread(true);
   joint_pos_errs_.resize(module_joint_num_);
   prev_morphing_stamp_ = ros::Time::now().toSec();
+  for(int i = 0; i < getMaxModuleNum(); i++){
+    std::string module_name  = string("/") + getMyName() + std::to_string(i+1);
+    module_joints_subs_.insert(make_pair(module_name, nh_.subscribe( module_name + string("/dock_joints_pos"), 1, &NinjaNavigator::moduleJointsCallback, this)));
+    KDL::JntArray joints_pos(2);
+    all_modules_joints_pos_.insert(make_pair(i+1,joints_pos));
+  }
 }
 
 void NinjaNavigator::update()
 {
   updateEntSysState();
+
   if(ros::Time::now().toSec() - prev_morphing_stamp_ > morphing_process_interval_)
     {
       comRotationProcess();
@@ -393,6 +402,7 @@ void NinjaNavigator::convertTargetPosFromCoG2CoM()
     return;
   } else if((!pre_assembled_  && current_assembled) || (current_assembled && reconfig_flag)){ //assembly or reconfig process
     setTargetCoMPoseFromCurrState();
+    setTargetJointPosFromCurrState();
     ROS_INFO("switched");
     pre_assembled_ = current_assembled;
     return;
@@ -514,6 +524,20 @@ void NinjaNavigator::setTargetCoMPoseFromCurrState()
 
 }
 
+void NinjaNavigator::setTargetJointPosFromCurrState()
+{
+  for(auto & it:assembled_modules_data_)
+    {
+      ModuleData& data = it.second;
+      data.goal_joint_pos_ = all_modules_joints_pos_[data.id_];
+      data.des_joint_pos_ = all_modules_joints_pos_[data.id_];
+      data.first_joint_processed_time_[YAW] = -1.0;
+      data.first_joint_processed_time_[PITCH] = -1.0;
+      ROS_INFO_STREAM("Module" << data.id_ << "-> " << "yaw: "<<data.des_joint_pos_(YAW)<< ", pitch: "<<data.des_joint_pos_(PITCH));
+    }
+  calcCenterOfMoving();
+}
+
 void NinjaNavigator::setGoalCoMRotCallback(const spinal::DesireCoordConstPtr & msg)
 {
   setGoalComRot(KDL::Rotation::RPY(msg->roll, msg->pitch, msg->yaw));
@@ -611,6 +635,53 @@ void NinjaNavigator::assemblyJointPosCallback(const sensor_msgs::JointStateConst
   if(joint_send_flag) joint_control_pub_.publish(joints_ctrl_msg);
 }
 
+void NinjaNavigator::jointStateCallback(const sensor_msgs::JointStateConstPtr& states)
+{
+  // send docking joint positions to other modules
+  sensor_msgs::JointState joint_states = *states;
+  sensor_msgs::JointState dock_joint_pos_msg;
+  for(int i =0; i< joint_states.name.size(); i++) {
+    if(joint_states.name[i] =="yaw_dock_joint")
+      {
+        dock_joint_pos_msg.name.push_back(string("mod")+std::to_string(getMyID())+string("/yaw"));
+        dock_joint_pos_msg.position.push_back(joint_states.position[i]);
+      }
+    else if(joint_states.name[i] =="pitch_dock_joint")
+      {
+        dock_joint_pos_msg.name.push_back(string("mod")+std::to_string(getMyID())+string("/pitch"));
+        dock_joint_pos_msg.position.push_back(joint_states.position[i]);
+      }
+  }
+  dock_joints_pos_pub_.publish(dock_joint_pos_msg);
+}
+
+void NinjaNavigator::moduleJointsCallback(const sensor_msgs::JointStateConstPtr& state)
+{
+  sensor_msgs::JointState joint_states = *state;
+  for(int i=0; i<joint_states.name.size(); i++)
+    {
+      // Extract id and joint name
+      int id;
+      std::string joint_name;
+      size_t pos = joint_states.name[i].find('/');
+
+      std::string module_name = joint_states.name[i].substr(0, pos);
+      joint_name = joint_states.name[i].substr(pos + 1);
+      std::regex numberRegex("\\d+");
+      std::smatch match;
+      if (std::regex_search(module_name, match, numberRegex))
+        id = std::stoi(match.str(0));
+      else
+        return;
+      KDL::JntArray& module_joint_pos = all_modules_joints_pos_[id];
+      int axis;
+      if(joint_name == "yaw") axis = YAW;
+      if(joint_name == "pitch") axis = PITCH;
+      module_joint_pos(axis) = joint_states.position[i];
+    }
+}
+
+
 void NinjaNavigator::morphingProcess()
 {
   if(!getCurrentAssembled() || !control_flag_) return;