class BSCtrl_TargetFollowing(RobotControllerAbstract): def __init__(self, registered_tf_buffer=None): super(BSCtrl_TargetFollowing, self).__init__( TwistStamped, rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC), registered_tf_buffer ) self.base_pose_ref = Pose(orientation=Quaternion(w=1.0)) self.base_pose_cur = Pose(orientation=Quaternion(w=1.0)) self.base_twist_ref = Twist() self.base_twist_cur = Twist() self.theta_timeline = TimeLineData().initialize_values(length=10) def activate_controller(self): self._navi_ref_subscriber = rospy.Subscriber( rospy.get_param(PARAM_NAME_NAVI_REF_TOPIC), NavigationReference, self._navi_ref_callback ) self._robot_state_subscriber = rospy.Subscriber( rospy.get_param(PARAM_NAME_ROBOT_STATE_TOPIC), RobotState, self._robot_state_callback ) return True def update_input(self): v, w = self._calc_pd_input() return TwistStamped(header=Header(stamp=rospy.Time.now()), twist=twist_from_vw(v, w)) def _calc_pd_input(self): Kp, Kd = 1, 1 theta = euler_from_rosquaternion(self.base_pose_cur.orientation)[0] R = np.matrix([[np.cos(theta), np.sin(theta)], [-np.sin(theta), np.cos(theta)]]) Xd_dot = np.matrix([[self.base_twist_ref.linear.x], [self.base_twist_ref.linear.y]]) Xd = np.matrix([[self.base_pose_ref.position.x], [self.base_pose_ref.position.y]]) X = np.matrix([[self.base_pose_cur.position.x], [self.base_pose_cur.position.y]]) u = R * (Kd * Xd_dot - Kp * (X - Xd)) v, w = float(u[0]), float(u[1]) return v, w def _calc_target_tracking_input(self): def dist_bw_points(pos1, pos2): return ((pos1.x - pos2.x) ** 2 + (pos1.y - pos2.y) ** 2 + (pos1.z - pos2.z) ** 2) ** 0.5 def yaw_bw_orientations(ori1, ori2): return euler_from_rosquaternion(ori1)[0] - euler_from_rosquaternion(ori2)[0] Kv, Kt, KtD = 0.1, 1, 1 self.theta_timeline.append(yaw_bw_orientations(self.base_pose_cur.orientation, self.base_pose_ref.orientation)) v = Kv * dist_bw_points(self.base_pose_ref.position, self.base_pose_cur.position) w = Kt * self.theta_timeline.dval + KtD * self.theta_timeline.differentiation print(v, w) return v, w def _navi_ref_callback(self, navi_ref): self.base_pose_ref = navi_ref.base_pose self.base_twist_ref = navi_ref.base_twist def _robot_state_callback(self, robot_state): self.base_pose_cur = robot_state.base_pose self.base_twist_cur = robot_state.base_twist
def __init__(self, registered_tf_buffer=None): super(BSCtrl_TargetFollowing, self).__init__( TwistStamped, rospy.get_param(PARAM_NAME_BS_INPUT_TOPIC), registered_tf_buffer ) self.base_pose_ref = Pose(orientation=Quaternion(w=1.0)) self.base_pose_cur = Pose(orientation=Quaternion(w=1.0)) self.base_twist_ref = Twist() self.base_twist_cur = Twist() self.theta_timeline = TimeLineData().initialize_values(length=10)