diff --git a/rm_gimbal_controllers/src/gimbal_base.cpp b/rm_gimbal_controllers/src/gimbal_base.cpp index 2c10a4df..e98d628b 100644 --- a/rm_gimbal_controllers/src/gimbal_base.cpp +++ b/rm_gimbal_controllers/src/gimbal_base.cpp @@ -112,6 +112,7 @@ bool Controller::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& ro ctrls_.insert(std::make_pair(axis, std::make_unique())); pid_pos_.insert(std::make_pair(axis, std::make_unique())); + pos_des_in_limit_.insert(std::make_pair(axis, true)); pos_state_pub_.insert(std::make_pair( axis, std::make_unique>(nh, "pos_state", 1))); @@ -207,7 +208,7 @@ void Controller::setDes(const ros::Time& time, double yaw_des, double pitch_des) odom2gimbal_des.setRPY(0, pitch_des, yaw_des); tf2::Quaternion base2gimbal_des = odom2base.inverse() * odom2gimbal_des; for (const auto& it : joint_urdfs_) - pos_des_in_limit_.insert(std::make_pair(it.first, setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des))); + pos_des_in_limit_[it.first] = setDesIntoLimit(base2gimbal_des, it.second, base2gimbal_des); odom2gimbal_des_.transform.rotation = tf2::toMsg(odom2base * base2gimbal_des); odom2gimbal_des_.header.stamp = time; robot_state_handle_.setTransform(odom2gimbal_des_, "rm_gimbal_controllers");