Skip to content

Commit

Permalink
Merge pull request #175 from liyixin135/master
Browse files Browse the repository at this point in the history
Initialize odom2pitch_des in rate through starting
  • Loading branch information
d0h0s authored Aug 2, 2024
2 parents 5a71357 + dc4e9f4 commit eb8bb7f
Show file tree
Hide file tree
Showing 2 changed files with 10 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -204,6 +204,7 @@ class Controller : public controller_interface::MultiInterfaceController<rm_cont
TRAJ
};
int state_ = RATE;
bool start_ = false;
};

} // namespace rm_gimbal_controllers
22 changes: 9 additions & 13 deletions rm_gimbal_controllers/src/gimbal_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -148,22 +148,11 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro
return true;
}

void Controller::starting(const ros::Time& time)
void Controller::starting(const ros::Time& /*unused*/)
{
state_ = RATE;
state_changed_ = true;
try
{
odom2pitch_ = robot_state_handle_.lookupTransform("odom", pitch_joint_urdf_->child_link_name, time);
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
return;
}
odom2gimbal_des_.transform.rotation = odom2pitch_.transform.rotation;
odom2gimbal_des_.header.stamp = time;
robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");
start_ = true;
}

void Controller::update(const ros::Time& time, const ros::Duration& period)
Expand Down Expand Up @@ -266,6 +255,13 @@ void Controller::rate(const ros::Time& time, const ros::Duration& period)
{ // on enter
state_changed_ = false;
ROS_INFO("[Gimbal] Enter RATE");
if (start_)
{
odom2gimbal_des_.transform.rotation = odom2pitch_.transform.rotation;
odom2gimbal_des_.header.stamp = time;
robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");
start_ = false;
}
}
else
{
Expand Down

0 comments on commit eb8bb7f

Please sign in to comment.