Skip to content

Commit

Permalink
[Ninja][Navigation] add featrue to wait center_of_movin tf for a while
Browse files Browse the repository at this point in the history
  • Loading branch information
sugihara-16 committed Sep 25, 2024
1 parent 63b2d12 commit c20a2fe
Showing 1 changed file with 2 additions and 0 deletions.
2 changes: 2 additions & 0 deletions robots/ninja/src/ninja_navigation.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@ void NinjaNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
joint_control_pub_ = nh_.advertise<sensor_msgs::JointState>("joints_ctrl", 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);
tfBuffer_.setUsingDedicatedThread(true);
joint_pos_errs_.resize(module_joint_num_);
prev_morphing_stamp_ = ros::Time::now().toSec();
}
Expand Down Expand Up @@ -479,6 +480,7 @@ void NinjaNavigator::setTargetCoMPoseFromCurrState()
if(current_assembled){
try
{
tfBuffer_.canTransform("world", my_name_ + std::to_string(my_id_) + std::string("/center_of_moving"), ros::Time::now(), ros::Duration(0.1));
KDL::Frame current_com;
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer_.lookupTransform("world", my_name_ + std::to_string(my_id_) + std::string("/center_of_moving") , ros::Time(0));
Expand Down

0 comments on commit c20a2fe

Please sign in to comment.