def _visualize_vel(self, p, v):
pt = cf.p_numpy_to_ros(p)
pt2 = cf.p_numpy_to_ros(v + p)
points = [pt, pt2]
self._pub_visualize.pub_velocity(points)
def _visualize_x(self, pose):
# current orientation
q_c = cf.q_ros_to_numpy(self._local_pose.pose.orientation)
# body frame x
x_b = np.array([1.0, 0.0, 0.0])
# convert to world frame
x = np.dot(cf.rotation_from_q_transpose(q_c), x_b)
pt = cf.p_numpy_to_ros(pose)
pt2 = cf.p_numpy_to_ros(pose + x)
pts = [pt, pt2]
self._pub_visualize.pub_x_vec(pts)
def circle(self, radius, normal, ax_body):
# assign values
self._radius = radius
self._normal = normal / np.linalg.norm(normal)
# current orientation in nunmpy form
q = f.q_ros_to_numpy(self._state.driver.local_pose.pose.orientation)
# axis to world frame: w = R_T * b
ax_world = np.transpose(
np.dot(np.transpose(f.rotation_from_q(q)), np.transpose(ax_body)))
#print "ax: ", ax_world
# point from current position + ax_world
pose = f.p_ros_to_numpy(self._state.driver.local_pose.pose.position)
point = pose + ax_world
# center: closest point to "point" and "circle"
self._center = cf.closest_to_circle_and_point(point, pose, normal,
radius)
#print "center: ", self._center
# vector from center to pose
delta_center_pose = np.transpose(pose - self._center)
point = pose
# angle from 0: 2 * pi
thetas = np.linspace(0.0, 2.0 * np.pi, 20)
# create points on circle path
q = np.array([0.0, 0.0, 0.0, 0.0]) # inialize rotation quaternion
points = [0.0 for i in thetas] # initialze points
self.pts = [0.0 for i in points]
for idx, theta in enumerate(thetas):
# create rotation quaternion
q[:3] = np.sin(theta * 0.5) * self._normal
q[3] = np.cos(theta * 0.5)
# new point is rotated vector + center
point_new = np.dot(f.rotation_from_q(q),
delta_center_pose) + self._center
# fill array
points[idx] = f.p_numpy_to_ros(point_new)
self.pts[idx] = point_new
# creat bezier points
self.bezier_points()
# send path
self.pub.pub_path(points)
def run(self):
self._doFollow = True
# get first bezier
bezier = self._path.bz_pt[0]
idx_next = 1
# set state into velocity ctr
self._state.set_state("bezier")
# init setpoints
self._path.pub.init_sp()
while self._doFollow:
# get current position
pc = cf.p_ros_to_numpy(self._state.driver.local_pose.pose.position)
# get t of bezier point closest to current posiition
pd, vd, ad = bf.point_closest_to_bezier(bezier, pc)
# publsih set point
self._path.pub.pub_setpoints(cf.p_numpy_to_ros(pd))
# check if pd is close to last point on bezier
if np.linalg.norm(pd - bezier[2]) <= 0.05:
bezier = self._path.bz_pt[idx_next]
idx_next += 1
if idx_next == len(self._path.bz_pt):
self._doFollow = False
# compute adjusted vel
#v_adj = self._vel_mapping(pd, vd, pc)
# send desired velocity
self._state.set_msg(bezier)
# go into position ctrl
self._state.set_state("posctr")
# stay at current position
self._state.set_msg(np.append(pc, np.array([0.0, 0.0, 1.0, 0.0])))
def _pub_a_desired(self):
# get current position, velocity
pose = cf.p_ros_to_numpy(self._local_pose.pose.position)
velocity = cf.p_ros_to_numpy(self._local_vel.twist.linear)
bz = [cf.p_ros_to_numpy(self._bezier_pt[0]), \
cf.p_ros_to_numpy(self._bezier_pt[1]), \
cf.p_ros_to_numpy(self._bezier_pt[2])]
# get closest point and velocity and acceleration to bezier
p_des, v_des, a_des = bf.point_closest_to_bezier(
bz, pose, self._bezier_duration)
# get desired velocity
#a_final = bf.accel_adjusted(p_des, v_des, a_des, pose, velocity)
print a_des
#print "a_des : {}\t v_des : {}\t p_des: {}".format(a_des, v_des, p_des)
# get yaw angle error
'''theta = 0.0
v_des_norm= np.linalg.norm(v_des)
z = np.array([0.0,0.0,1.0])
if (v_des_norm > 0.0) and not (np.array_equal(np.abs(v_des/v_des_norm), z)): #yaw not defined if norm(v_des) or v_des == z
theta = self.angle_error(v_des)'''
#a_final = np.array([0.0,0.0,0.52])
# assign to msg
self._accel_msg.vector = cf.p_numpy_to_ros(a_des)
# publish
self._accel_pub.publish(self._accel_msg)
def __init__(self, nh):
self._run_bz_controller = False
# vel pub
self._vel_pub = rospy.Publisher('mavros/setpoint_velocity/cmd_vel',
TwistStamped,
queue_size=10)
self._vel_msg = TwistStamped()
# acc pub
self._accel_pub = rospy.Publisher('/mavros/setpoint_accel/accel',
Vector3Stamped,
queue_size=10)
self._accel_msg = Vector3Stamped()
# attitude
self._att_pub = rospy.Publisher('/mavros/setpoint_raw/attitude',
AttitudeTarget,
queue_size=10)
self._att_msg = AttitudeTarget()
self._att_msg.type_mask = 7
# local raw: send acceleration and yaw
self._acc_yaw_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
self._acc_yaw_msg = PositionTarget()
self._acc_yaw_msg.type_mask = 2048 + 32 + 16 + 8 + 4 + 2 + 1 #+ 512
# local raw: send velocity and yaw
self._vel_yaw_pub = rospy.Publisher('/mavros/setpoint_raw/local',
PositionTarget,
queue_size=10)
self._vel_yaw_msg = PositionTarget()
self._vel_yaw_msg.type_mask = 1 + 2 + 4 + 64 + 128 + 256 + 2048
# path bezier triplet send
self._bezier_triplet_pub = rospy.Publisher('/mavros/avoidance_triplet',
AvoidanceTriplet,
queue_size=10)
self._bezier_triplet_msg = AvoidanceTriplet()
self._bezier_duration = 1.0
# initlaize publisher for visualization
self._pub_visualize = pub_bezier.pub_bezier()
# dt
self._dt = 0.0
# call back variables
self._pid_coeff = pidCoeff()
#print self._pid_coeff
# pid tuning parameters with first callback
Server(PIDConfig, self._pidcallback)
self._ctr = controller.controller(self._pid_coeff, 9.91)
### subscriber ###
# state subscriber
self._rate_state = rospy.Rate(RATE_STATE)
self._current_state = State()
rospy.Subscriber('/mavros/state', State, self._current_state_cb)
# subscriber,
self._local_pose = PoseStamped()
self._local_pose.pose.position = cf.p_numpy_to_ros([0.0, 0.0, 0.0])
rospy.Subscriber('/mavros/local_position/pose', PoseStamped,
self._local_pose_cb)
self._local_vel = TwistStamped()
self._local_vel.twist.linear = cf.p_numpy_to_ros([0.0, 0.0, 0.0])
rospy.Subscriber('/mavros/local_position/velocity', TwistStamped,
self._local_vel_cb)
self._bezier_pt = []
'''self._bezier_pt[0] = cf.p_numpy_to_ros([0.0,0.0,0.0])
self._bezier_pt[1] = cf.p_numpy_to_ros([0.0,0.0,0.0])
self._bezier_pt[2] = cf.p_numpy_to_ros([0.0,0.0,0.0]'''
#self._bezier_duration = 1.0
rospy.Subscriber('/path/bezier_pt', Path, self._bezier_cb)
rospy.Subscriber('/path/three_point_message', ThreePointMsg,
self._three_point_msg_cb)
self._linear_acc = Vector3()
self._linear_acc = cf.p_numpy_to_ros_vector([0.0, 0.0, 0.0])
rospy.Subscriber('/mavros/imu/data', Imu, self._imu_cb)
def _visualize_acc(self, p, a):
pt = cf.p_numpy_to_ros(p)
pt2 = cf.p_numpy_to_ros(p + a)
points = [pt, pt2]
self._pub_visualize.pub_a_vec(points)
def _visualize_target(self, p):
pt = cf.p_numpy_to_ros(p)
self._pub_visualize.pub_target(pt)