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)