def quaternion_product(q1, q2):
q_prod = Quaternion()
a1 = q1.w
b1 = q1.x
c1 = q1.y
d1 = q1.z
a2 = q2.w
b2 = q2.x
c2 = q2.y
d2 = q2.z
q_prod.w = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2
q_prod.x = a1 * b2 + b1 * a2 + c1 * d2 - d1 * c2
q_prod.y = a1 * c2 - b1 * d2 + c1 * a2 + d1 * b2
q_prod.z = a1 * d2 + b1 * c2 - c1 * b2 + d1 * a2
# This is exactly the same quaternion as before, but with positive
# real part (seems to be the standard for ROS TF)
if q_prod.w < 0.0:
q_prod.x = -q_prod.x
q_prod.y = -q_prod.y
q_prod.z = -q_prod.z
q_prod.w = -q_prod.w
return q_prod
def sense(self):
""" collects sensor data from the Neato and publishes
it to the neatoSensors topic
"""
# receive and publish sensor data
self.fields = self.neato.state.keys() # update sensor fields
sensor_data = NeatoSensors() # see NOTE above
for field in self.fields:
try:
sensor_data.__setattr__(field, self.neato.state[field])
except:
pass
self.sensorPub.publish(sensor_data)
# receive and publish laser range data
range_data = LaserRangeData()
range_data.__setattr__("range_data", self.neato.range_data)
self.rangePub.publish(range_data)
# odomemtry in testing
self.odomUpdate()
# transform position into tf frame
quaternionOdom = Quaternion()
quaternionOdom.x = 0.0
quaternionOdom.y = 0.0
quaternionOdom.z = sin(self.theta/2)
quaternionOdom.w = -cos(self.theta/2)
quaternionLL = Quaternion()
quaternionLL.x = 0.0
quaternionLL.y = 0.0
quaternionLL.z = 0.0
quaternionLL.w = 1.0
# base_link -> base_laser transformation
self.odomBroadcaster.sendTransform(
(0.0, -0.1, 0.0),
(quaternionLL.x, quaternionLL.y, quaternionLL.z, quaternionLL.w),
rospy.Time.now(),
"/base_laser",
"/base_link")
# odom -> base_link transformation
self.odomBroadcaster.sendTransform(
(self.x/1000, self.y/1000, 0),
(quaternionOdom.x, quaternionOdom.y, quaternionOdom.z, quaternionOdom.w),
rospy.Time.now(),
"/base_link",
"/odom")
def Publish(self, broadcaster, orientation, x_dist, y_dist, linear_speed, angular_speed, use_pose_ekf=False):
ros_now = rospy.Time.now()
quat = Quaternion()
# Orientation can be one of two things:
# Euler Angles or Quaternion
# Orientation is a dictionary keyed by 'euler' and 'quaternion'
if orientation.has_key('euler'):
euler = orientation['euler']
# Note: Euler values are in degrees
roll = euler['roll']
pitch = euler['pitch']
yaw = euler['yaw']
q = transformations.quaternion_from_euler(roll, pitch, yaw)
quat.x = q[0]
quat.y = q[1]
quat.z = q[2]
quat.w = q[3]
elif orientation.has_key('quaternion'):
quat.x = orientation['quaternion']['x']
quat.y = orientation['quaternion']['y']
quat.z = orientation['quaternion']['z']
quat.w = orientation['quaternion']['w']
# Publish the transform from frame odom to frame base_link over tf
# Note: pose ekf is how turtlebot does its thing. If there is an imu and/or gyro, it might be best to take
# this approach
if use_pose_ekf:
# This transform conflicts with transforms built into the Turtle stack
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20broadcaster%20%28Python%29
# This is done in/with the robot_pose_ekf because it can integrate IMU/gyro data
# using an "extended Kalman filter"
# REMOVE this "line" if you use robot_pose_ekf
pass
else:
broadcaster.sendTransform(
(x_dist, y_dist, 0),
(quat.x, quat.y, quat.z, quat.w),
ros_now,
"base_footprint",
"odom"
)
self._publisher.publish(self._msg(quat, x_dist, y_dist, linear_speed, angular_speed, ros_now, use_pose_ekf))
def list_to_dict(lst):
"""
Convert a list to a pose dictionary, assuming it is in the order
x, y, z, orientation x, orientation y, orientation z[, orientation w].
"""
if len(lst) == 6:
qtn = tf.transformations.quaternion_from_euler(lst[3], lst[4], lst[5])
elif len(lst) == 7:
qtn = Quaternion()
qtn.x = lst[3]
qtn.y = lst[4]
qtn.z = lst[5]
qtn.w = lst[6]
else:
raise MoveItCommanderException("""Expected either 6 or 7 elements
in list: (x,y,z,r,p,y) or (x,y,z,qx,qy,qz,qw)""")
pnt = Point()
pnt.x = lst[0]
pnt.y = lst[1]
pnt.z = lst[2]
pose_dict = {
'position': pnt,
'orientation': qtn
}
return pose_dict
def axis_angle_to_quaternion(axis, angle):
q = Quaternion()
q.x = axis.x * math.sin(angle / 2.0)
q.y = axis.y * math.sin(angle / 2.0)
q.z = axis.z * math.sin(angle / 2.0)
q.w = math.cos(angle / 2.0)
return q
def rotateQuaternion(q_orig, yaw):
"""
Converts a basic rotation about the z-axis (in radians) into the
Quaternion notation required by ROS transform and pose messages.
:Args:
| q_orig (geometry_msgs.msg.Quaternion): to be rotated
| yaw (double): rotate by this amount in radians
:Return:
| (geometry_msgs.msg.Quaternion) q_orig rotated yaw about the z axis
"""
# Create a temporary Quaternion to represent the change in heading
q_headingChange = Quaternion()
p = 0
y = yaw / 2.0
r = 0
sinp = math.sin(p)
siny = math.sin(y)
sinr = math.sin(r)
cosp = math.cos(p)
cosy = math.cos(y)
cosr = math.cos(r)
q_headingChange.x = sinr * cosp * cosy - cosr * sinp * siny
q_headingChange.y = cosr * sinp * cosy + sinr * cosp * siny
q_headingChange.z = cosr * cosp * siny - sinr * sinp * cosy
q_headingChange.w = cosr * cosp * cosy + sinr * sinp * siny
# Multiply new (heading-only) quaternion by the existing (pitch and bank)
# quaternion. Order is important! Original orientation is the second
# argument rotation which will be applied to the quaternion is the first
# argument.
return multiply_quaternions(q_headingChange, q_orig)
def Publish_Odom_Tf(self): #quaternion = tf.transformations.quaternion_from_euler(0, 0, theta) quaternion = Quaternion() quaternion.x = 0 quaternion.y = 0 quaternion.z = math.sin(self.Heading / 2.0) quaternion.w = math.cos(self.Heading / 2.0) rosNow = rospy.Time.now() self._tf_broad_caster.sendTransform( (self.X_Pos, self.Y_Pos, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), rosNow, "base_footprint", "odom" ) # next, we'll publish the odometry message over ROS odometry = Odometry() odometry.header.frame_id = "odom" odometry.header.stamp = rosNow odometry.pose.pose.position.x = self.X_Pos odometry.pose.pose.position.y = self.Y_Pos odometry.pose.pose.position.z = 0 odometry.pose.pose.orientation = quaternion odometry.child_frame_id = "base_link" odometry.twist.twist.linear.x = self.Velocity odometry.twist.twist.linear.y = 0 odometry.twist.twist.angular.z = self.Omega self._odom_publisher.publish(odometry)
def get_ros_quaternion(self, almath_quaternion):
output = Quaternion()
output.w = almath_quaternion.w
output.x = almath_quaternion.x
output.y = almath_quaternion.y
output.z = almath_quaternion.z
return output
def getOrientation(self):
ort = Quaternion()
ort.x = self._orientationX
ort.y = self._orientationY
ort.z = self._orientationZ
ort.w = self._orientationW
return ort
def poll(self):
(x, y, theta) = self.arduino.arbot_read_odometry()
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
# Create the odometry transform frame broadcaster.
now = rospy.Time.now()
self.odomBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
now,
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = x
odom.pose.pose.position.y = y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.forwardSpeed
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.angularSpeed
self.arduino.arbot_set_velocity(self.forwardSpeed, self.angularSpeed)
self.odomPub.publish(odom)
def publish(self, data):
if not self._calib and data.getImuMsgId() == PUB_ID:
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
elif data.getImuMsgId() == CALIB_ID:
x, y, z = data.getValues()
msg = imuCalib()
if x > self._xMax:
self._xMax = x
if x < self._xMin:
self._xMin = x
if y > self._yMax:
self._yMax = y
if y < self._yMin:
self._yMin = y
if z > self._zMax:
self._zMax = z
if z < self._zMin:
self._zMin = z
msg.x.data = x
msg.x.max = self._xMax
msg.x.min = self._xMin
msg.y.data = y
msg.y.max = self._yMax
msg.y.min = self._yMin
msg.z.data = z
msg.z.max = self._zMax
msg.z.min = self._zMin
self._pubCalib.publish(msg)
def __init__(self):
position=Point()
rotation=Quaternion()
rospy.init_node('tf_listener')
self.pub = rospy.Publisher('robot_odom', robot_odom, queue_size=10)
self.now=rospy.Time()
self.tf_listener=tf.TransformListener()
rospy.sleep(0.5)
frame='/odom'
wantedframe = self.frame_checker(frame)
while not rospy.is_shutdown():
(po,ro)=self.get_odom(frame,wantedframe)
position.x=po[0]
position.y=po[1]
position.z=po[2]
rotation.x=ro[0]
rotation.y=ro[1]
rotation.z=ro[2]
rotation.w=ro[3]
#rospy.sleep()
self.publish(position,rotation,wantedframe)
#print 'position: \n',position,'\n','rotation: \n',self.quat_to_angle(rotation),'time: ', self.robot_odom.header.stamp
print self.robot_odom,'\nangular: ',self.quat_to_angle(self.robot_odom.rotation)#self.normalize_angle(self.quat_to_angle(self.robot_odom.rotation))
def convert_planar_phi_to_quaternion(phi):
quaternion = Quaternion()
quaternion.x = 0
quaternion.y = 0
quaternion.z = math.sin(phi / 2)
quaternion.w = math.cos(phi / 2)
return quaternion
def update_kalman(ar_tags):
print "started update"
rospy.wait_for_service('innovation')
update = rospy.ServiceProxy('innovation', NuSrv)
listener = tf.TransformListener()
while True:
try:
try:
(trans, rot) = listener.lookupTransform(ar_tags['arZ'], ar_tags['ar1'], rospy.Time(0))
except:
print "Couldn't look up transform"
continue
lin = Vector3()
quat = Quaternion()
lin.x = trans[0]
lin.y = trans[1]
lin.z = trans[2]
quat.x = rot[0]
quat.y = rot[1]
quat.z = rot[2]
quat.w = rot[3]
transform = Transform()
transform.translation = lin
transform.rotation = quat
test = update(transform, ar_tags['ar1'])
print "Service call succeeded"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def quaternion_to_msg(q):
msg = Quaternion()
msg.x = q[0]
msg.y = q[1]
msg.z = q[2]
msg.w = q[3]
return msg
def array_to_quaternion(nparr):
quat = Quaternion()
quat.x = nparr[0]
quat.y = nparr[1]
quat.z = nparr[2]
quat.w = nparr[3]
return quat
def compare_all(data):
global k
last_row = csv_data[k-frequency_offset]
diff = Quaternion()
diff.x = round(float(last_row[1])-convert_ppm(data.channels[3]),4)
diff.y = round(float(last_row[2])-convert_ppm(data.channels[1]),4)
diff.z = round(float(last_row[4])-convert_ppm(data.channels[0]),4)
diff.w = round(float(last_row[5])+convert_ppm(data.channels[2]),4)
testing_pub.publish(diff)
if (k < len(csv_data)):
row = csv_data[k]
#log for post processing
#joystick log
c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/gps_joy.csv", "ab"))
c.writerow([data.header.stamp.secs,row[1],row[2],row[4],row[5]])
#rc Inn log
c = csv.writer(open("/home/jinahadam/catkin_ws/src/testing/rc_in.csv", "ab"))
c.writerow([data.header.stamp.secs,convert_ppm(data.channels[3]),convert_ppm(data.channels[1]),convert_ppm(data.channels[0]),convert_ppm(data.channels[2])])
msg = Joy()
msg.axes = (float(row[1]),float(row[2]),float(row[3]),float(row[4]),float(row[5]),float(row[6]),float(row[7]))
msg.buttons = (0,0,0,0,0,0,0,0,0,0,0)
k = k + 1
pub.publish(msg)
else:
#k = 0
print("Joystick & RC In data logged with GPS Time")
exit()
def publish(self, data):
q = data.getOrientation()
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "Before ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
array = quaternion_from_euler(roll, pitch, yaw + (self._angle * pi / 180))
res = Quaternion()
res.w = array[0]
res.x = array[1]
res.y = array[2]
res.z = array[3]
roll, pitch, yaw = euler_from_quaternion([q.w, q.x, q.y, q.z])
# print "after ", "Yaw: " + str(yaw * 180 / pi), "Roll: " + str(roll * 180 / pi), "pitch: " + str(pitch * 180 / pi), "\n\n"
msg = Imu()
msg.header.frame_id = self._frameId
msg.header.stamp = rospy.get_rostime()
msg.orientation = res
msg.linear_acceleration = data.getAcceleration()
msg.angular_velocity = data.getVelocity()
magMsg = MagneticField()
magMsg.header.frame_id = self._frameId
magMsg.header.stamp = rospy.get_rostime()
magMsg.magnetic_field = data.getMagnetometer()
self._pub.publish(msg)
self._pubMag.publish(magMsg)
def get_nearest_cloud(self, msg):
points = list()
points_xy = list()
# Get all the points in the visible cloud (may be prefiltered by other nodes)
for point in point_cloud2.read_points(msg, skip_nans=True):
points.append(point[:3])
points_xy.append(point[:2])
# Convert to a numpy array
points_arr = np.float32([p for p in points]).reshape(-1, 1, 3)
# Compute the COG
cog = np.mean(points_arr, 0)
# Convert to a Point
cog_point = Point()
cog_point.x = cog[0][0]
cog_point.y = cog[0][1]
cog_point.z = cog[0][2]
#cog_point.z = 0.35
# Abort if we get an NaN in any component
if np.isnan(np.sum(cog)):
return
# If we have enough points, find the best fit ellipse around them
try:
if len(points_xy) > 6:
points_xy_arr = np.float32([p for p in points_xy]).reshape(-1, 1, 2)
track_box = cv2.fitEllipse(points_xy_arr)
else:
# Otherwise, find the best fitting rectangle
track_box = cv2.boundingRect(points_xy_arr)
angle = pi - radians(track_box[2])
except:
return
#print angle
# Convert the rotation angle to a quaternion
q_angle = quaternion_from_euler(0, angle, 0, axes='sxyz')
q = Quaternion(*q_angle)
q.x = 0.707
q.y = 0
q.z = 0.707
q.w = 0
# Publish the COG and orientation
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = msg.header.frame_id
target.pose.position = cog_point
target.pose.orientation = q
# Publish the movement command
self.target_pub.publish(target)
def quat_product (qa,qb): qc=Quaternion() qc.w = qa.w*qb.w - (qa.x*qb.x + qa.y*qb.y + qa.z*qb.z) qc.x = (qa.w*qb.x + qa.x*qb.w + qa.y*qb.z - qa.z*qb.y) qc.y = (qa.w*qb.y - qa.x*qb.z + qa.y*qb.w + qa.z*qb.x) qc.z = (qa.w*qb.z + qa.x*qb.y - qa.y*qb.x + qa.z*qb.w) return qc
def publish(self, data):
q = Quaternion()
q.x = 0
q.y = 0
q.z = data[6]
q.w = data[7]
odomMsg = Odometry()
odomMsg.header.frame_id = self._odom
odomMsg.header.stamp = rospy.get_rostime()
odomMsg.child_frame_id = self._baseLink
odomMsg.pose.pose.position.x = data[0]
odomMsg.pose.pose.position.y = data[1]
odomMsg.pose.pose.position.z = 0
odomMsg.pose.pose.orientation = q
if self._firstTimePub:
self._prevOdom = odomMsg
self._firstTimePub = False
return
velocity = Twist()
deltaTime = odomMsg.header.stamp.to_sec() - self._prevOdom.header.stamp.to_sec()
yaw, pitch, roll = euler_from_quaternion(
[odomMsg.pose.pose.orientation.w, odomMsg.pose.pose.orientation.x, odomMsg.pose.pose.orientation.y,
odomMsg.pose.pose.orientation.z])
prevYaw, prevPitch, prevRollprevYaw = euler_from_quaternion(
[self._prevOdom.pose.pose.orientation.w, self._prevOdom.pose.pose.orientation.x,
self._prevOdom.pose.pose.orientation.y, self._prevOdom.pose.pose.orientation.z])
if deltaTime > 0:
velocity.linear.x = (data[8] / deltaTime)
deltaYaw = yaw - prevYaw
# rospy.loginfo("yaw: %f\t\tpevYaw: %f\t\tdeltaYaw: %f" % (yaw,prevYaw,deltaYaw))
if deltaYaw > math.pi: deltaYaw -= 2 * math.pi
elif deltaYaw < -math.pi: deltaYaw += 2 * math.pi
if deltaTime > 0:
velocity.angular.z = -(deltaYaw / deltaTime)
# rospy.loginfo("deltaYaw after check: %f\t\t angular: %f" % (deltaYaw, velocity.angular.z))
odomMsg.twist.twist = velocity
self._prevOdom = odomMsg
traMsg = TransformStamped()
traMsg.header.frame_id = self._odom
traMsg.header.stamp = rospy.get_rostime()
traMsg.child_frame_id = self._baseLink
traMsg.transform.translation.x = data[0]
traMsg.transform.translation.y = data[1]
traMsg.transform.translation.z = 0
traMsg.transform.rotation = q
self._pub.publish(odomMsg)
self._broadCase.sendTransformMessage(traMsg)
def run(self):
rosRate = rospy.Rate(self.rate)
rospy.loginfo("Publishing Odometry data at: " + str(self.rate) + " Hz")
vx = 0.0
vy = 0.0
vth = 0.0
while not rospy.is_shutdown() and not self.finished.isSet():
now = rospy.Time.now()
if now > self.t_next:
dt = now - self.then
self.then = now
dt = dt.to_sec()
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
"base_link",
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
self.t_next = now + self.t_delta
rosRate.sleep()
def advance(self, dt):
now = self.then + dt
elapsed = now - self.then
self.then = now
if self.debug:
print "advancing to t=%5.3f" % (self.then)
ldist = dt * self.lwSpeed
rdist = dt * self.rwSpeed
d = (ldist + rdist)/2.0
th = (rdist - ldist)/self.distanceBetweenWheels
dx = d / dt
dth = th / dt
if (d != 0):
x = math.cos(th)*d
y = math.sin(th)*d # Does this differ from the create driver? Should it?
self.x = self.x + (math.cos(self.th)*x - math.sin(self.th)*y)
self.y = self.y + (math.sin(self.th)*x + math.cos(self.th)*y)
if (th != 0):
self.th = self.th + th
self.th = self.th % (2 * math.pi)
self.th = self.th - (2 * math.pi) if self.th > math.pi else self.th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(self.th/2.0)
quaternion.w = math.cos(self.th/2.0)
# Most of the applications I'm working on don't need this.
# self.odomBroadcaster.sendTransform(
# (self.x, self.y, 0),
# (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
# rospy.Time.now(),
# "base_link",
# "odom"
# )
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = "base_link"
odom.twist.twist.linear.x = dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth
self.odomPub.publish(odom)
def quat_normalize(q): q1=Quaternion() qnorm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z) q1.w = q.w/qnorm q1.x = q.x/qnorm q1.y = q.y/qnorm q1.z = q.z/qnorm return q1
def only_yaw(self, quat, mag=1, add=0):
_, _, yaw = euler_from_quaternion(self.quat_to_list(quat))
quat_yaw = quaternion_from_euler(0.0, 0.0, mag * yaw + add)
ret_quat = Quaternion()
ret_quat.x = quat_yaw[0]
ret_quat.y = quat_yaw[1]
ret_quat.z = quat_yaw[2]
ret_quat.w = quat_yaw[3]
return ret_quat
def quat_inverse (q): q_inv=Quaternion() norm = sqrt(q.w*q.w + q.x*q.x + q.y*q.y + q.z*q.z) q_inv.w = +q.w / norm q_inv.x = -q.x / norm q_inv.y = -q.y / norm q_inv.z = -q.z / norm return q_inv
def _x_y_yaw_to_pose(self, x, y, yaw):
position = Point(x, y, 0)
orientation = Quaternion()
q = quaternion_from_euler(0, 0, yaw)
orientation.x = q[0]
orientation.y = q[1]
orientation.z = q[2]
orientation.w = q[3]
return Pose(position=position, orientation=orientation)
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# this approximation works (in radians) for small angles
th = self.th - self.th_pre
self.dr = th / elapsed
# publish the odom information
quaternion = Quaternion()
quaternion.x = self.qx
quaternion.y = self.qy
quaternion.z = self.qz
quaternion.w = self.qw
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(0, 0, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id,
)
self.laserBroadcaster.sendTransform(
(0, 0, 0), (0, 0, 0, 1), rospy.Time.now(), self.laser_frame_id, self.base_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
laser = LaserScan()
laser.header.stamp = now
laser.header.frame_id = self.laser_frame_id
laser.angle_min = self.laser.angle_min
laser.angle_max = self.laser.angle_max
laser.angle_increment = self.laser.angle_increment
laser.time_increment = self.laser.time_increment
laser.scan_time = self.laser.scan_time
laser.range_min = self.laser.range_min
laser.range_max = self.laser.range_max
laser.ranges = self.laser.ranges
laser.intensities = self.laser.intensities
self.laserPub.publish(laser)
def publish_odometry(self):
quat = tf.transformations.quaternion_from_euler(0, 0, self.yaw)
quaternion = Quaternion()
quaternion.x = quat[0]
quaternion.y = quat[1]
quaternion.z = quat[2]
quaternion.w = quat[3]
if self.last_time is None:
self.last_time = rospy.Time.now()
vx = self.vx #(msg->twist.twist.linear.x) *trans_mul_;
vy = self.vy #msg->twist.twist.linear.y;
vth = self.vyaw #(msg->twist.twist.angular.z) *rot_mul_;
current_time = rospy.Time.now()
dt = (current_time - self.last_time).to_sec()
delta_x = (vx * math.cos(self.yaw) - vy * math.sin(self.yaw)) * dt
delta_y = (vx * math.sin(self.yaw) + vy * math.cos(self.yaw)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.yaw += delta_th
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = "odom"
odom.child_frame_id = "base_link"
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
self.odom_pub.publish(odom)
transform_stamped = TransformStamped()
transform_stamped.header = odom.header
transform_stamped.transform.translation.x = self.x
transform_stamped.transform.translation.y = self.y
transform_stamped.transform.translation.z = 0.0
transform_stamped.transform.rotation = quaternion
#self.tfbr.sendTransform(transform_stamped)
self.tfbr.sendTransform((self.x, self.y, 0.0),
(quat[0], quat[1], quat[2], quat[3]),
rospy.Time.now(),
"odom",
"base_link")
self.last_time = current_time
def quaternion_conjugate(q):
q_conj = Quaternion()
q_conj.x = -q.x
q_conj.y = -q.y
q_conj.z = -q.z
q_conj.w = q.w
return q_conj
msg.header.frame_id = IMU_FRAME_ID
mag = Vector3()
mag.x = data["compass"][0] / 1000000
mag.y = data["compass"][1] / 1000000
mag.z = data["compass"][2] / 1000000
msg.magnetic_field = mag
quat = Quaternion()
"""
quat.x = data["fusionQPose"][0]
quat.y = data["fusionQPose"][1]
quat.z = data["fusionQPose"][2]
quat.w = data["fusionQPose"][3]
"""
quat.x = q[1]
quat.y = q[2]
quat.z = q[3]
quat.w = q[0]
imu_dat = Imu()
imu_dat.header.frame_id = IMU_FRAME_ID
imu_dat.header.stamp = rospy.Time.now()
imu_dat.linear_acceleration = acc
imu_dat.linear_acceleration_covariance = [0.04, 0, 0,
0, 0.04, 0,
0, 0, 0.04]
imu_dat.orientation = quat
imu_dat.orientation_covariance = [0.0025, 0, 0,
0, 0.0025, 0,
0, 0, 0.0025]
imu_dat.angular_velocity = gyro
def poll(self):
now = rospy.Time.now()
t_delta = now - self.last_poll
v_step = self.acc_lim * t_delta.to_sec()
vdelta_step = self.acc_lim_diff * t_delta.to_sec()
# read battery status
battVoltage = self.arduino.get_battery()
# rospy.loginfo("battery: " + str(battVoltage) + " V")
if battVoltage < 11.0 and now > (
self.last_batt_warning +
rospy.Duration(self.batt_warning_rate)):
#rospy.loginfo("batt warning, voltage: " + str(battVoltage) + " V")
self.ttsPub.publish("My battery is running low")
self.last_batt_warning = now
# Read the odometry
pose = self.arduino.get_pose() ###
self.x = pose[0]
self.y = pose[1]
self.theta = pose[2]
self.v = pose[3]
self.dtheta = pose[4]
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.theta / 2.0)
quaternion.w = cos(self.theta / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), now,
self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.frame_id = self.odom_frame_id
odom.child_frame_id = self.base_frame_id
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = self.v
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dtheta
self.odomPub.publish(odom)
self.sonarDist = self.arduino.get_sonar_distance() ###
sonar = Range()
sonar.header.frame_id = self.sonar_frame_id
sonar.header.stamp = now
sonar.radiation_type = sonar.ULTRASOUND
sonar.field_of_view = 15.0 * math.pi / 180.0
sonar.min_range = 0.02
sonar.max_range = 0.50
sonar.range = self.sonarDist
self.sonarPub.publish(sonar)
# if cmd_vel subscription timed out
if now > (self.last_cmd_vel + rospy.Duration(self.cmd_vel_timeout)):
self.v_des = 0
self.dtheta_des = 0
# calculate commanded velocities
if self.v_des > self.v_cmd:
self.v_cmd += v_step
if self.v_des < self.v_cmd:
self.v_cmd = self.v_des
else:
if self.v_des < self.v_cmd:
self.v_cmd -= v_step
if self.v_des > self.v_cmd:
self.v_cmd = self.v_des
if 0.454 * self.dtheta_des > self.vdelta_cmd:
self.vdelta_cmd += vdelta_step
if 0.454 * self.dtheta_des < self.vdelta_cmd:
self.vdelta_cmd = 0.454 * self.dtheta_des
else:
if 0.454 * self.dtheta_des < self.vdelta_cmd:
self.vdelta_cmd -= vdelta_step
if 0.454 * self.dtheta_des > self.vdelta_cmd:
self.vdelta_cmd = 0.454 * self.dtheta_des
vleft_cmd = self.v_cmd - self.vdelta_cmd / 2.0
vright_cmd = self.v_cmd + self.vdelta_cmd / 2.0
#rospy.loginfo("dtheta_des: " + str(self.dtheta_des) + " rad/s\tdtheta " + str(self.dtheta) + " rad/s\tvdelta_cmd " + str(self.vdelta_cmd) + " m/s")
#rospy.loginfo("vleft_cmd: " + str(vleft_cmd) + " m/s\tvright_cmd " + str(vright_cmd) + " m/s")
# Set motor speeds
if not self.stopped:
self.arduino.motor_command(vleft_cmd, vright_cmd) ###
self.last_poll = now
from geometry_msgs.msg import Vector3
import string
from env_mission.msg import detect_obj
if __name__ == '__main__':
pub = rospy.Publisher('detect_obj', detect_obj, queue_size=10)
rospy.init_node('dectection_publisher')
tmp = detect_obj()
pose = Pose()
point = Point()
point.x = 2.2
point.y = 3.2
point.z = 4.2
quat = Quaternion()
quat.x = 1.23
quat.y = 24.56
quat.z = 4.5
quat.w = 1.0
vec = Vector3()
vec.x = 1.0
vec.y = 2.0
vec.z = 3.0
pose.position = point
pose.orientation = quat
rate = rospy.Rate(0.5)
while not rospy.is_shutdown():
tmp.mission = 'start'
tmp.objname = 'tree'
tmp.objid = '2'
tmp.objpose = pose
udp.receiver()
# set publish data
# odometry = Float32MultiArray()
# odometry.data = (float(udp.view3Recv[1]) / 10, float(udp.view3Recv[2]) / 10, float(udp.view3Recv[3]) / 10)
checkFlag = float(udp.view3Recv[1])
getOdometry = Odometry()
poseWithCovariance = PoseWithCovariance()
point = Point()
quaternion = Quaternion()
pose = Pose()
header = Header()
point.x = float(udp.view3Recv[2]) / 10
point.y = float(udp.view3Recv[3]) / 10
point.z = float(udp.view3Recv[4]) / 10
quaternion.x = 0
quaternion.y = 0
quaternion.z = 0
quaternion.w = 0
pose.position = point
pose.orientation = quaternion
poseWithCovariance.pose = pose
poseWithCovariance.covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
header.seq = 1
header.stamp = rospy.Time.now()
header.frame_id = "odometry"
getOdometry.header = header
getOdometry.pose = poseWithCovariance
velocity = Float32MultiArray()
velocity.data = (float(udp.view3Recv[5]) / 10, float(udp.view3Recv[6]) / 10, float(udp.view3Recv[7]) / 10)
# rospy.loginfo(getOdometry)
def poll(self):
if self.debugPID:
try:
left_pidin, right_pidin = self.arduino.get_pidin()
self.LeftEncoderPub.publish(left_pidin)
self.RightEncoderPub.publish(right_pidin)
except:
rospy.logerr("getpidin exception count :")
return
try:
left_pidout, right_pidout = self.arduino.get_pidout()
self.LeftPidoutPub.publish(left_pidout)
self.RightPidoutPub.publish(right_pidout)
except:
rospy.logerr("getpidout exception count :")
return
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)*self.linear_scale_correction
self.y += (sin(self.th) * dx + cos(self.th) * dy)*self.linear_scale_correction
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0 *self.angular_scale_correction)
quaternion.w = cos(self.th / 2.0 *self.angular_scale_correction)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame,
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x*self.linear_scale_correction
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
if self.debugPID:
self.LeftVelPub.publish(self.v_left)
self.RightVelPub.publish(self.v_right)
self.t_next = now + self.t_delta
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
try:
awheel_enc, bwheel_enc, cwheel_enc, dwheel_enc = self.arduino.get_encoder_counts(
)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_A == None and self.enc_B == None and self.enc_C == None and self.enc_D == None:
d_A = 0
d_B = 0
d_C = 0
d_D = 0
else:
d_A = (awheel_enc - self.enc_A) / self.ticks_per_meter
d_B = (bwheel_enc - self.enc_B) / self.ticks_per_meter
d_C = (cwheel_enc - self.enc_C) / self.ticks_per_meter
d_D = (dwheel_enc - self.enc_D) / self.ticks_per_meter
self.enc_A = awheel_enc
self.enc_B = bwheel_enc
self.enc_C = cwheel_enc
self.enc_D = dwheel_enc
va = d_A / dt
vb = d_B / dt
vc = d_C / dt
vd = d_D / dt
# va = w(R-L) = vc = v - wL
# vb = w(R+L) = vd = v + wL
# L = wheel_track / 2.0
vx = (va + vb + vc + vd) / 4.0
vth = (vb - va + vd - vc) / (2.0 * self.wheel_track)
dx = cos(vth * dt) * vx * dt
dy = -sin(vth * dt) * vx * dt
self.x += cos(self.th) * dx - sin(self.th) * dy
self.y += sin(self.th) * dx + sin(self.th) * dy
self.th += vth * dt
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_AWheel = 0
self.v_des_BWheel = 0
self.v_des_CWheel = 0
self.v_des_DWheel = 0
if self.v_A < self.v_des_AWheel:
self.v_A += self.max_accel
if self.v_A > self.v_des_AWheel:
self.v_A = self.v_des_AWheel
else:
self.v_A -= self.max_accel
if self.v_A < self.v_des_AWheel:
self.v_A = self.v_des_AWheel
if self.v_B < self.v_des_BWheel:
self.v_B += self.max_accel
if self.v_B > self.v_des_BWheel:
self.v_B = self.v_des_BWheel
else:
self.v_B -= self.max_accel
if self.v_B < self.v_des_BWheel:
self.v_B = self.v_des_BWheel
if self.v_C < self.v_des_CWheel:
self.v_C += self.max_accel
if self.v_C > self.v_des_CWheel:
self.v_C = self.v_des_CWheel
else:
self.v_C -= self.max_accel
if self.v_C < self.v_des_CWheel:
self.v_C = self.v_des_CWheel
if self.v_D < self.v_des_DWheel:
self.v_D += self.max_accel
if self.v_D > self.v_des_DWheel:
self.v_D = self.v_des_DWheel
else:
self.v_D -= self.max_accel
if self.v_D < self.v_des_DWheel:
self.v_D = self.v_des_DWheel
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_A, self.v_B, self.v_C, self.v_D)
# if self.debugPID:
# self.AVelPub.publish(self.v_A)
# self.BVelPub.publish(self.v_B)
# self.CVelPub.publish(self.v_C)
# self.DVelPub.publish(self.v_D)
self.t_next = now + self.t_delta
#!/usr/bin/env python
import roslib
roslib.load_manifest('spacepoint')
import rospy, sys
import spacepoint
from geometry_msgs.msg import Quaternion
print "hello world"
if __name__ == '__main__':
rospy.init_node('spacepoint')
quat_pub = rospy.Publisher('spacepoint_quat', Quaternion)
fusion = spacepoint.SpacePoint()
q = Quaternion()
while not rospy.is_shutdown():
#print repr(fusion)
fusion.update()
q.x = fusion.quat[0]
q.y = fusion.quat[1]
q.z = fusion.quat[2]
q.w = fusion.quat[3]
quat_pub.publish(q)
def update(self):
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
if self.fake:
x = cos(self.th) * self.dx * elapsed
y = -sin(self.th) * self.dx * elapsed
self.x += cos(self.th) * self.dx * elapsed
self.y += sin(self.th) * self.dx * elapsed
self.th += self.dr * elapsed
else:
# read encoders
try:
left, right = self.status()
except Exception as e:
rospy.logerr("Could not update encoders: " + str(e))
return
rospy.logdebug("Encoders: " + str(left) + "," + str(right))
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (left - self.enc_left) / self.ticks_meter
d_right = (right - self.enc_right) / self.ticks_meter
self.enc_left = left
self.enc_right = right
d = (d_left + d_right) / 2
th = (d_right - d_left) / self.base_width
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
x = cos(th) * d
y = -sin(th) * d
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
# publish or perish
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
if now > (self.last_cmd + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
# update motors
if not self.fake:
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.write(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def poll(self):
now = rospy.Time.now()
# 是否到更新时间
if now > self.t_next: # 到达 控制 时间 周期
#读取 编码器的值 反解运动控制模型 得到 机器人的里程记信息
try:
left_enc, right_enc = self.arduino.get_encoder_counts(
) # 两个轮子的 当前编码器计数
except:
self.bad_encoder_count += 1 # 不正常 编码器计数
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then # 时间差
self.then = now # 更新上次时间
dt = dt.to_sec() # 转换到秒
# 计算里程记信息（机器人自身 轮子的速度信息）
if self.enc_left == None: # 没有读取到编码器信息
dright = 0
dleft = 0
else:
dright = (right_enc -
self.enc_right) / self.ticks_per_meter # 右轮 速度
dleft = (left_enc -
self.enc_left) / self.ticks_per_meter # 左轮 速度
self.enc_right = right_enc #更新 编码器计数
self.enc_left = left_enc
# 两轮车 运动模型 反解 三轮车 不一样
dxy_ave = (dright + dleft) / 2.0 # 平均速度
dth = (dright -
dleft) / self.wheel_track # 速度差（弧长）/（转弯半径，即两轮间距） 转弯角度
vxy = dxy_ave / dt # 线速度大小
vth = dth / dt # 角速度大小
# 得到位置信息
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy) #当前位置
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
# 四元素信息
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# 发布坐标变换信息
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
# 发布里程记 信息（位置 四元素 速度（线速度+角速度）
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.odomPub.publish(odom)
# 根据期望的速度（cmd_vel上的命令） 向arduino发送控制速度信息（逐步调整命令速度到 下位机单片机，单片机在用pid控制算法是速度精确跟随上位机发布的指令）
#期望的轮子速度（v_des_left、v_des_right）会在 订阅话题"cmd_vel"节点的回调函数cmdVelCallback里得到更新
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0 #超过读取时间了，没有速度信息发布到话题上，即为 停止状态
self.v_des_right = 0
# 左轮子速度逐步调整置 期望左轮速度
if self.v_left < self.v_des_left: # 当前速度小于期望速度 则增加
self.v_left += self.max_accel # + 增量
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left # 最大限制（最大不超过期望速度）
else: # 当前速度大于期望速度 则减小
self.v_left -= self.max_accel # - 增量
if self.v_left < self.v_des_left: # 最小限制（最小不低于期望速度）
self.v_left = self.v_des_left
# 右轮子速度逐步调整置 期望右轮速度
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# 向下位机发送期望的轮子速度 根据编码器的反馈信息使用 PID 控制跟随期望速度
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta # 下次循环时间
def start(self):
state = None
goal = None
n_t_t = None
check_angle = 0
while True:
# Check if someone changed self.goal from outside
if goal != self.goal:
goal = copy.deepcopy(self.goal)
state = 'turn'
speed = 0.2
start = copy.deepcopy(self._base._current_odom)
desired_distance = utils.calc_minkowski_distance(
start.pose.pose.position, goal.position, 2)
n_t_t = utils.angular_diff(
self._base._current_odom.pose.pose.position, goal.position)
# print 'Need_to_turn: {}'.format(n_t_t)
"""
if utils.calculate_necessary_yaw(self._base._current_odom.pose.pose.orientation, goal.orientation) < 0:
n_t_t *= -1
"""
# print 'Need_to_turn: {}'.format(n_t_t)
'''# print 'REQ_DIST: {}'.format(desired_distance)
# print 'goal.oriant'
# print goal.orientation.w
# print 'current oriant'
# print self._base._current_odom.pose.pose.orientation.w
'''
if state == 'turn':
# TODO: Compute how much we need to turn to face the goal
"""
required_angular_distance = utils.calculate_necessary_yaw(self._base._current_odom.pose.pose.orientation, goal.orientation)
"""
"""
current_pos = self._base._current_odom.pose.pose.position
current_yaw = utils.quaternion_to_yaw(self._base._current_odom.pose.pose.orientation)
target_orientation = utils.calc_angl(current_pos, goal.position)
# print "Target Orientation: {}".format(target_orientation / 3.1415)
# print "Orientation Diff: {}".format(target_orientation - current_yaw)
"""
need_orientaion = Quaternion()
need_orientaion.x = 0
need_orientaion.y = 0
need_orientaion.z = 0
need_orientaion.w = 0
required_angular_distance = utils.calculate_necessary_yaw(
self._base._current_odom.pose.pose.orientation,
need_orientaion)
#n_t_t = math.pi / 2 - utils.angular_diff(self._base._current_odom.pose.pose.position, goal.position)
required_angular_distance = required_angular_distance - n_t_t
if required_angular_distance < -math.pi:
required_angular_distance += 2 * math.pi
# print 'REQ_ANG_DIST: {}'.format(required_angular_distance)
rotate_speed = 1.0
if required_angular_distance > 0:
rotate_speed *= -1
if abs(required_angular_distance) > 0.6:
self._base.move(0, rotate_speed)
elif abs(required_angular_distance) > 0.35:
rotate_speed *= 0.6
self._base.move(0, rotate_speed)
elif abs(required_angular_distance) > 0.04:
rotate_speed *= 0.3
self._base.move(0, rotate_speed)
else:
# print 'Done Turning'
speed = 0.4
self._base.stop()
time.sleep(0.5)
state = 'move'
if state == 'move':
# Compute how far we have moved and compare that to desired_distance
# TODO: Make sure that the robot has the ability to drive backwards if it overshoots
check_angle += 1
curr_distance = utils.calc_minkowski_distance(
start.pose.pose.position,
self._base._current_odom.pose.pose.position, 2)
delta = curr_distance - desired_distance
if curr_distance < desired_distance:
# print 'delta: {}'.format(delta)
if abs(delta) < 0.5:
speed = 0.2
self._base.move(speed, 0)
# print 'check_turn: {}'.format(check_angle)
if check_angle % 80 == 0:
state = 'turn'
else:
self._base.stop()
time.sleep(0.5)
state = 'final'
if state == 'final':
required_angular_distance = utils.calculate_necessary_yaw(
self._base._current_odom.pose.pose.orientation,
goal.orientation)
if required_angular_distance < -math.pi:
required_angular_distance += 2 * math.pi
rotate_speed = 1.0
if required_angular_distance > 0:
rotate_speed *= -1
if abs(required_angular_distance) > 0.6:
self._base.move(0, rotate_speed)
elif abs(required_angular_distance) > 0.35:
rotate_speed *= 0.6
self._base.move(0, rotate_speed)
elif abs(required_angular_distance) > 0.04:
rotate_speed *= 0.3
self._base.move(0, rotate_speed)
else:
# print 'Done Turning'
speed = 0.5
self._base.stop
time.sleep(0.5)
state = None
rospy.sleep(0.1)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
if (self.useSonar == True) :
try:
r0, r1, r2, r3, r4= self.arduino.ping()
rospy.loginfo("r0: " + str(r0)+"r1: " + str(r1) + "r2: " + str(r2) + "r3: " + str(r3)+ "r4: " + str(r4))
#rospy.loginfo("r0: " + str(r0) + "r3: " + str(r3))
sonar0_range = Range()
sonar0_range.header.stamp = now
sonar0_range.header.frame_id = "/sonar0"
sonar0_range.radiation_type = Range.ULTRASOUND
sonar0_range.min_range = 0.05
sonar0_range.max_range = 4.0
sonar0_range.range = r0/100.0
self.sonar0_pub.publish(sonar0_range)
sonar1_range = Range()
sonar1_range.header.stamp = now
sonar1_range.header.frame_id = "/sonar1"
sonar1_range.radiation_type = Range.ULTRASOUND
sonar1_range.min_range = 0.05
sonar1_range.max_range = 4.0
sonar1_range.range = r1/100.0
self.sonar1_pub.publish(sonar1_range)
sonar2_range = Range()
sonar2_range.header.stamp = now
sonar2_range.header.frame_id = "/sonar2"
sonar2_range.radiation_type = Range.ULTRASOUND
sonar2_range.min_range = 0.05
sonar2_range.max_range = 4.0
sonar2_range.range = r2/100.0
self.sonar2_pub.publish(sonar2_range)
sonar3_range = Range()
sonar3_range.header.stamp = now
sonar3_range.header.frame_id = "/sonar3"
sonar3_range.radiation_type = Range.ULTRASOUND
sonar3_range.min_range = 0.05
sonar3_range.max_range = 4.0
sonar3_range.range = r3/100.0
self.sonar3_pub.publish(sonar3_range)
sonar4_range = Range()
sonar4_range.header.stamp = now
sonar4_range.header.frame_id = "/sonar4"
sonar4_range.radiation_type = Range.ULTRASOUND
sonar4_range.min_range = 0.05
sonar4_range.max_range = 4.0
sonar4_range.range = r4/100.0
self.sonar4_pub.publish(sonar4_range)
if(sonar0_range.range>=0.05) and (sonar0_range.range<=4.0):
self.front_ranger_l = sonar0_range.range
else:
self.front_ranger_l = 10.0
if(sonar1_range.range>=0.05) and (sonar1_range.range<=4.0):
self.front_ranger_r = sonar1_range.range
else:
self.front_ranger_r = 10.0
except:
self.front_ranger_l = 10.0
self.front_ranger_r = 10.0
self.bad_encoder_count += 1
rospy.logerr("ping exception count: " + str(self.bad_encoder_count))
return
try:
self.voltage_val = self.arduino.get_voltage()*10
#print "voltage_val=",self.voltage_val
self.voltage_pub.publish(self.voltage_val)
#print "publish voltage_val is",self.voltage_val
except:
self.voltage_pub.publish(-1)
#rospy.logerr("get voltage value error")
#return
try:
self.emergencybt_val = self.arduino.get_emergency_button()
#print "emergencybt_val=",self.emergencybt_val
self.emergencybt_pub.publish(self.emergencybt_val)
#print "publish emergencybt_val is",self.emergencybt_val
except:
self.emergencybt_pub.publish(-1)
#rospy.logerr("get emergencybt status error")
#return
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
#rospy.loginfo("left_enc: " + str(left_enc)+"right_enc: " + str(right_enc))
self.lEncoderPub.publish(left_enc)
self.rEncoderPub.publish(right_enc)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
if (left_enc < self.encoder_low_wrap and self.enc_left > self.encoder_high_wrap) :
self.l_wheel_mult = self.l_wheel_mult + 1
elif (left_enc > self.encoder_high_wrap and self.enc_left < self.encoder_low_wrap) :
self.l_wheel_mult = self.l_wheel_mult - 1
else:
self.l_wheel_mult = 0
if (right_enc < self.encoder_low_wrap and self.enc_right > self.encoder_high_wrap) :
self.r_wheel_mult = self.r_wheel_mult + 1
elif (right_enc > self.encoder_high_wrap and self.enc_right < self.encoder_low_wrap) :
self.r_wheel_mult = self.r_wheel_mult - 1
else:
self.r_wheel_mult = 0
#dright = (right_enc - self.enc_right) / self.ticks_per_meter
#dleft = (left_enc - self.enc_left) / self.ticks_per_meter
dleft = 1.0 * (left_enc + self.l_wheel_mult * (self.encoder_max - self.encoder_min)-self.enc_left) / self.ticks_per_meter
dright = 1.0 * (right_enc + self.r_wheel_mult * (self.encoder_max - self.encoder_min)-self.enc_right) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
if (self.useImu == False) :
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame,
"odom"
)
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
# todo sensor_state.distance == 0
#if self.v_des_left == 0 and self.v_des_right == 0:
# odom.pose.covariance = ODOM_POSE_COVARIANCE2
# odom.twist.covariance = ODOM_TWIST_COVARIANCE2
#else:
# odom.pose.covariance = ODOM_POSE_COVARIANCE
# odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def local_callback(local):
global drone_pose
drone_pose = local
def set_goal_rotation(theta, (x, y, z)):
global goal_rotation
theta = math.pi * theta / 360
if x * x + y * y + z * z != 1:
norm = math.sqrt((x * x) + (y * yo) + (z * z))
x /= norm
y /= norm
z /= norm
goal_rotation.x = math.sin(theta) * x
goal_rotation.y = math.sin(theta) * y
goal_rotation.z = math.sin(theta) * z
goal_rotation.w = math.cos(theta)
def set_relative_orientation(theta, (x, y, z)):
global goal_rotation
theta = math.pi * theta / 360
if x * x + y * y + z * z != 1:
norm = math.sqrt((x * x) + (y * yo) + (z * z))
x /= norm
y /= norm
z /= norm
relative_rotation = Quaternion()
relative_rotation.x = math.sin(theta) * x
relative_rotation.y = math.sin(theta) * y
def talker(self):
#hello_str = "hello world %s" % rospy.get_time()
#rospy.loginfo(hello_str)
#pub.publish(hello_str)
#print self.sock.recv(1024)
len1 = 17
buf = self.sock.recv(1024)
if buf:
#data17 = repr(buf)
#for i in range(0,11):
#print ('%x'%repr(buf))
print(
"==========================start================================="
)
self.current_time = rospy.get_time() #记录获得数据的当前时刻
timer = rospy.get_time()
print(self.current_time)
dt = self.current_time - self.last_time
self.last_time = self.current_time
#合法性判断todo （1）长度为17个字节，（2） data[16] 为data[3]~data[15]的异或
#合法则进行下面的工作
h0, h1, len2, uid, Encoder1, Encoder2, Encoder3, checksum = struct.unpack(
"!4c3ic", buf)
#记录编码器数据的增减
Encoder1_diff = Encoder1 - self.last_Encoder1
self.last_Encoder1 = Encoder1
Encoder2_diff = Encoder2 - self.last_Encoder2
self.last_Encoder2 = Encoder2
Encoder3_diff = Encoder3 - self.last_Encoder3
self.last_Encoder3 = Encoder3
#根据编码器读数转换为角速度 wtheta1，wtheta2，wtheta3
Ticks = 1224 #厂家提供实测一圈（2×math.pi弧度）对应的编码器点数
wrad1 = math.pi * 2 * Encoder1_diff / Ticks #Encoder1_diff点数对应的弧度数
wrad2 = math.pi * 2 * Encoder2_diff / Ticks #Encoder1_diff点数对应的弧度数
wrad3 = math.pi * 2 * Encoder3_diff / Ticks #Encoder1_diff点数对应的弧度数
wtheta1 = wrad1 / dt #角速度,rad/s
wtheta2 = wrad2 / dt #角速度,rad/s
wtheta3 = wrad3 / dt #角速度,rad/s
#转化为vx,vy,vth
angle = math.pi / 6
sina = 0.5 #math.sin(angle)
cosa = math.cos(angle)
tt = (wtheta1 + wtheta2 - 2 * wtheta3) / (2 + 2 * sina)
self.vx = self.radius * (wtheta2 - wtheta1) / (2 * cosa)
self.vy = self.radius * tt
self.vth = (wtheta3 + tt) * self.radius / self.L
if (self.vx != 0) or (self.vy != 0):
delta_x = self.vx * dt
delta_y = self.vy * dt
# calculate the final position of the robot
self.x = self.x + (math.cos(self.th) * delta_x -
math.sin(self.th) * delta_y)
self.y = self.y + (math.sin(self.th) * delta_x +
math.cos(self.th) * delta_y)
if (self.vth != 0):
delta_th = self.vth * dt
self.th = self.th + delta_th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = math.sin(self.th / 2)
quaternion.w = math.cos(self.th / 2)
self.odom_broadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
now = rospy.Time.now()
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.vx
odom.twist.twist.linear.y = self.vy
odom.twist.twist.angular.z = self.vth
self.odom_pub.publish(odom)
print(dt)
#self.odom_broadcaster.sendTransform((self.x, self.y, 0),
# tf.transformations.quaternion_from_euler(0, 0, self.th),
# rospy.Time.now(),
# "base_link",
# "odom")
#self.tf_listener = tf.TransformListener()
#if self.tf_listener.waitForTransform(self.odom_frame_id, self.base_frame_id, rospy.Time(0),rospy.Duration(5.0)):
#print ("1")
self.rate.sleep()
def __init__(self):
rospy.init_node('read_serial')
# Initialize the serial class
ser = serial.Serial()
# Define the serial port
ser.port = "/dev/ttyACM0"
# Set the baudrate
ser.baudrate = 115200
# Set the timeout limit
ser.timeout = 1
# Open the serial
ser.open()
quat_upper_pub = rospy.Publisher('upperarm_quaternion',
Quaternion,
queue_size=10)
quat_should_pub = rospy.Publisher('shoulder_quaternion',
Quaternion,
queue_size=10)
quat_fore_pub = rospy.Publisher('forearm_quaternion',
Quaternion,
queue_size=10)
# Initialize the flag used to check the availability of the received serial data
flag_sent = 0
rate = rospy.Rate(40.0)
while not rospy.is_shutdown():
try:
# Read data from the Serial
data = ser.read(buffLen)
# data = ser.readline()
# print "len:"
# print len(data)
#print(data[2])
# print(len(data))
if (len(data) >= buffLen):
parse_data(data)
quat_sent = Quaternion()
quat_sent.w = quat_total[0][0]
quat_sent.x = quat_total[0][1]
quat_sent.y = quat_total[0][2]
quat_sent.z = quat_total[0][3]
quat_fore_pub.publish(quat_sent)
quat_sent = Quaternion()
quat_sent.w = quat_total[1][0]
quat_sent.x = quat_total[1][1]
quat_sent.y = quat_total[1][2]
quat_sent.z = quat_total[1][3]
quat_should_pub.publish(quat_sent)
quat_sent = Quaternion()
quat_sent.w = quat_total[2][0]
quat_sent.x = quat_total[2][1]
quat_sent.y = quat_total[2][2]
quat_sent.z = quat_total[2][3]
quat_upper_pub.publish(quat_sent)
# # Split the serial data into several words separated by space
# words = data.split()
# # Get the length of the data array
# word_len = len(words)
# rate.sleep()
except KeyboardInterrupt:
ser.close()
print("Bye")
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = (d_left + d_right) / 2
# this approximation works (in radians) for small angles
th = (d_right - d_left) / self.base_width
# calculate velocities
# Guard against elapsed being 0
if elapsed == 0:
elapsed = 0.000000001
# Velocities
self.dx = d / elapsed
self.dr = th / elapsed
# This if velocity in the y direction of the base link that is
# present if there is an offset from the wheel center to the base_link
# center. It is the y velocity induced on the body through angular
# velocity
self.dy = sin(self.dr) * self.x_trans
if (d != 1):
# calculate distance traveled in x and y
x = cos(th) * d
y = -sin(th) * d
# calculate the final position of the robot
self.x = self.x + (cos(self.th) * x - sin(self.th) * y)
self.y = self.y + (sin(self.th) * x + cos(self.th) * y)
if (th != 0):
self.th = self.th + th
# build the Qauternion
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2)
quaternion.w = cos(self.th / 2)
# Convert the frame!
dx = cos(self.th) * self.x_trans
self.x2 = self.x + dx
dy = sin(self.th) * self.x_trans
self.y2 = self.y + dy
# rospy.loginfo("dx %f dy %f" % (dx, dy))
# rospy.loginfo(" x: %f \t y: %f \tth: %f" % (self.x, self.y, self.th))
# rospy.loginfo("x2: %f \ty2: %f \tth: %f" % (self.x2, self.y2, self.th))
# Publish the transform
if self.publish_tf:
self.odomBroadcaster.sendTransform(
(self.x2, self.y2, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame_id, self.odom_frame_id)
# Send over the odom
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x2
odom.pose.pose.position.y = self.y2
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = self.dy
odom.twist.twist.angular.z = self.dr
# Build the covariance matrix
odom.pose.covariance = self.ODOM_POSE_COVARIANCE
odom.twist.covariance = self.ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
def update(self):
#############################################################################
now = rospy.Time.now()
if now > self.t_next:
elapsed = now - self.then
self.then = now
elapsed = elapsed.to_sec()
# calculate odometry
if self.enc_left == None:
d_left = 0
d_right = 0
else:
d_left = (self.left - self.enc_left) / self.ticks_meter
d_right = (self.right - self.enc_right) / self.ticks_meter
self.enc_left = self.left
self.enc_right = self.right
# distance traveled is the average of the two wheels
d = ( d_left + d_right ) / 2
# this approximation works (in radians) for small angles
th = ( d_right - d_left ) / self.base_width
# calculate velocities
self.dx = d / elapsed
self.dr = th / elapsed
if (d != 0):
# calculate distance traveled in x and y
x = cos( th ) * d
y = -sin( th ) * d
# calculate the final position of the robot
self.x = self.x + ( cos( self.th ) * x - sin( self.th ) * y )
self.y = self.y + ( sin( self.th ) * x + cos( self.th ) * y )
if( th != 0):
self.th = self.th + th
# publish the odom information
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin( self.th / 2 )
quaternion.w = cos( self.th / 2 )
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(),
self.base_frame_id,
self.odom_frame_id
)
odom = Odometry()
odom.header.stamp = now
odom.header.frame_id = self.odom_frame_id
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.child_frame_id = self.base_frame_id
odom.twist.twist.linear.x = self.dx
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = self.dr
self.odomPub.publish(odom)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
except Exception as e:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count) + " now: " +
e.__class__.__name__)
rospy.logerr("mw stack trace: " + traceback.format_exc())
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dx = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
x = cos(dth) * dx
y = -sin(dth) * dx
self.x += (cos(self.th) * x - sin(self.th) * y)
self.y += (sin(self.th) * x + cos(self.th) * y)
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
odom = Odometry(header=rospy.Header(frame_id=self.odom_frame),
child_frame_id=self.base_frame)
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = dx / dt
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = dth / dt
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w), now,
self.base_frame, self.odom_frame)
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def compute_imu_msg(self):
"""
This method reads data from the Openlog Artemis output.
We receive 3 coord Quaternion, which causes discontinuities in rotation.
RPY coords are stored and reused to compute a new quaternion
"""
# Get data
# rtcDate,rtcTime,Q6_1,Q6_2,Q6_3,RawAX,RawAY,RawAZ,RawGX,RawGY,RawGZ,RawMX,RawMY,RawMZ,output_Hz,\r\n
try:
data = self.ser.readline().split(",")
except SerialException:
self.log("Error reading data from IMU", 2, alert="warn")
self.fails = self.fails + 1
if self.fails > 10:
self.connection()
return
else:
self.fails = 0
if len(data) < 16:
self.log("IMU Communication failed", 4, alert="warn")
return
# Compute Absolute Quaternion
try:
q = [float(data[2]), float(data[3]), float(data[4]), 0]
if ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])) > 1.0:
self.log("Inconsistent IMU readings", 4, alert="warn")
return
q[3] = np.sqrt(1.0 - ((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2])))
except ValueError:
self.log("Error converting IMU message - {}".format(data), 5, alert="warn")
return
# Compute Linear Acceleration
acc = [round(float(data[5])*self.acc_ratio, 3), round(float(data[6])*self.acc_ratio, 3), round(float(data[7])*self.acc_ratio, 3)]
self.acc_hist.pop(0)
self.acc_hist.append(acc)
# Compute Angular Velocity
# w_x = float(data[8])*3.14/180
# w_y = float(data[9])*3.14/180
# w_z = float(data[10])*3.14/180
# Compute Angular Velocity from Quat6
lq = self.last_imu[-1].orientation
euler1 = transformations.euler_from_quaternion([lq.x, lq.y, lq.z, lq.w])
euler2 = transformations.euler_from_quaternion(q)
curr_time = rospy.Time.now()
dt = (curr_time - self.last_imu[-1].header.stamp).to_sec()
w = []
for i in range(0, 3):
dth = euler2[i] - euler1[i]
# The IMU Quaternion jumps need to be handled
while (3.14 < dth) or (dth < -3.14):
dth = dth - np.sign(dth)*2*np.pi
# Keep euler angles in [-2p and 2pi]
self.euler[i] += dth
while (2*np.pi < self.euler[i]) or (self.euler[i] < -2*np.pi):
self.euler[i] = self.euler[i] - np.sign(self.euler[i])*2*np.pi
w.append(round(dth/dt, 4))
q_est = transformations.quaternion_from_euler(self.euler[0], self.euler[1], self.euler[2])
new_q = Quaternion()
new_q.x = q_est[0]
new_q.y = q_est[1]
new_q.z = q_est[2]
new_q.w = q_est[3]
# Compute IMU Msg
imu_msg = Imu()
imu_msg.header.frame_id = self.tf_prefix+"imu"
imu_msg.header.stamp = curr_time
imu_msg.orientation = new_q
# Set the sensor covariances
imu_msg.orientation_covariance = [
0.001, 0, 0,
0, 0.001, 0,
0, 0, 0.001
]
# Angular Velocity
imu_msg.angular_velocity.x = w[0]
imu_msg.angular_velocity.y = w[1]
imu_msg.angular_velocity.z = w[2]
# Datasheet says:
# - Noise Spectral Density: 0.015dps/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(0.015/np.sqrt(20), 2)
# factor = 0.02
# imu_msg.angular_velocity_covariance = [
# diag, w_x*factor, w_x*factor,
# w_y*factor, diag, w_y*factor,
# w_z*factor, w_z*factor, diag
# ]
imu_msg.angular_velocity_covariance = [0.0] * 9
imu_msg.angular_velocity_covariance[0] = -1
imu_msg.angular_velocity_covariance[0] = 0.01
imu_msg.angular_velocity_covariance[4] = 0.01
imu_msg.angular_velocity_covariance[8] = 0.05
# imu_msg.angular_velocity_covariance = [-1] * 9
# Linear Acceleration
acc = [0, 0, 0]
for idx in range(0, 3):
data = [a[idx] for a in self.acc_hist]
res = butter_lowpass_filter(data, cutoff=0.5, fs=10.0, order=1)
acc[idx] = res[-1]
imu_msg.linear_acceleration.x = acc[0]
imu_msg.linear_acceleration.y = acc[1]
imu_msg.linear_acceleration.z = acc[2]
# imu_msg.linear_acceleration.x = 0
# imu_msg.linear_acceleration.y = 0
# imu_msg.linear_acceleration.z = 9.82
# imu_msg.linear_acceleration_covariance = [-1] * 9
# Datasheet says:
# - Noise Spectral Density: 230microg/sqrt(Hz)
# - Cross Axis Sensitivy: +-2%
# diag = pow(230e-6/np.sqrt(20), 2)/256.
# factor = 0.02/256.
# imu_msg.linear_acceleration_covariance = [
# diag, acc_x*factor, acc_x*factor,
# acc_y*factor, diag, acc_y*factor,
# acc_z*factor, acc_z*factor, diag
# ]
imu_msg.linear_acceleration_covariance = [0.0] * 9
imu_msg.linear_acceleration_covariance[0] = 0.01
imu_msg.linear_acceleration_covariance[4] = 0.01
imu_msg.linear_acceleration_covariance[8] = 0.05
# Message publishing
self.imu_pub.publish(imu_msg)
new_q = imu_msg.orientation
[r, p, y] = transformations.euler_from_quaternion([new_q.x, new_q.y, new_q.z, new_q.w])
self.imu_euler_pub.publish("Roll: {} | Pitch: {} | Yaw: {}".format(r, p, y))
self.last_imu.pop(0)
self.last_imu.append(imu_msg)
def run_ros():
rospy.init_node('ros_demo')
# First param: topic name; second param: type of the message to be published; third param: size of queued messages,
# at least 1
chatter_pub = rospy.Publisher('some_chatter', String, queue_size=10)
marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
path_points_pub = rospy.Publisher('path_points', PoseArray, queue_size=10)
# Each second, ros "spins" and draws 20 frames
loop_rate = rospy.Rate(20) # 20hz
frame_count = 0
obstacles = create_obstacles()
obstacles_lines = get_obstacles_lines(obstacles)
robot = create_robot()
target = create_target()
target_lines = get_target_lines(target)
point = get_point_structure()
tree_edges = get_tree_edges_structure()
p0 = Point(robot.pose.position.x, robot.pose.position.y, 0)
tree = Tree(TreeNode(p0))
collision_edges = get_collision_edges_structure()
found_path = False
path_edges = get_path_edges_structure()
drawed_path = False
robot_reached = False
path_published = False
path_points = []
while not rospy.is_shutdown():
msg = "Frame index: %s" % frame_count
rospy.loginfo(msg)
chatter_pub.publish(msg)
for obst in obstacles:
obst.header.stamp = rospy.Time.now()
marker_pub.publish(obst)
robot.header.stamp = rospy.Time.now()
target.header.stamp = rospy.Time.now()
marker_pub.publish(target)
point.header.stamp = rospy.Time.now()
tree_edges.header.stamp = rospy.Time.now()
collision_edges.header.stamp = rospy.Time.now()
path_edges.header.stamp = rospy.Time.now()
if frame_count % HZ == 0 and not found_path:
rand_pnt = Point()
rand_pnt.x = random.uniform(BOARD_CORNERS[0], BOARD_CORNERS[1])
rand_pnt.y = random.uniform(BOARD_CORNERS[3], BOARD_CORNERS[2])
rand_pnt.z = 0
point.points = [rand_pnt]
close_node = tree.get_closest_node(rand_pnt)
close_pnt = close_node.point
# https://math.stackexchange.com/questions/175896/finding-a-point-along-a-line-a-certain-distance-away-from-another-point
total_dist = math.sqrt(
math.pow(rand_pnt.x - close_pnt.x, 2) +
math.pow(rand_pnt.y - close_pnt.y, 2))
dist_ratio = STEP / total_dist
new_pnt = Point()
new_pnt.x = (1 -
dist_ratio) * close_pnt.x + dist_ratio * rand_pnt.x
new_pnt.y = (1 -
dist_ratio) * close_pnt.y + dist_ratio * rand_pnt.y
new_pnt.z = 0
if collides_object(close_pnt, new_pnt, obstacles_lines):
collision_edges.points.append(close_pnt)
collision_edges.points.append(new_pnt)
else:
last_node = tree.add_node(close_node, new_pnt)
tree_edges.points.append(close_pnt)
tree_edges.points.append(new_pnt)
if collides_object(close_pnt, new_pnt, target_lines):
found_path = True
if found_path and not drawed_path:
current_node = last_node
while not current_node.is_root():
path_points.append(current_node.point)
path_edges.points.append(current_node.point)
path_edges.points.append(current_node.parent.point)
current_node = current_node.parent
drawed_path = True
if found_path and drawed_path and not path_published:
path_poses = []
for i in range(len(path_points) - 1):
current_point = path_points[i]
next_point = path_points[i + 1]
current_pose = Pose()
current_pose.position = current_point
current_quat = Quaternion()
current_quat.x = 0
current_quat.y = 0
current_quat.z = 1
prev_angle = 0
if i > 0:
prev_angle = path_poses[i - 1].orientation.w
current_quat.w = np.arctan(
(next_point.y - current_point.y) /
(next_point.x - current_point.x)) - prev_angle
current_pose.orientation = current_quat
path_poses.append(current_pose)
path_pose_array = PoseArray()
path_pose_array.poses = path_poses
path_points_pub.publish(path_pose_array)
path_published = True
if frame_count % 2 == 0 and drawed_path and not robot_reached:
robot.pose.position = path_points.pop()
robot_reached = True if len(path_points) == 0 else False
if robot_reached:
break
marker_pub.publish(robot)
marker_pub.publish(point)
marker_pub.publish(tree_edges)
marker_pub.publish(collision_edges)
marker_pub.publish(path_edges)
# TODO: Robot function
"""
# From here, we are defining and drawing a simple robot
rob.type = rob.SPHERE
path.type = path.LINE_STRIP
rob.header.frame_id = "map"
path.header.frame_id = "map"
rob.header.stamp = rospy.Time.now()
path.header.stamp = rospy.Time.now()
rob.ns = "rob"
path.ns = "rob"
rob.id = 0
path.id = 1
rob.action = rob.ADD
path.action = path.ADD
rob.lifetime = rospy.Duration()
path.lifetime = rospy.Duration()
rob.scale.x, rob.scale.y, rob.scale.z = 0.3, 0.3, 0.3
rob.color.r, rob.color.g, rob.color.b, rob.color.a = 1.0, 0.5, 0.5, 1.0
# Path line strip is blue
path.color.b, path.color.a = 1.0, 1.0
path.scale.x = 0.02
path.pose.orientation.w = 1.0
num_slice2 = 200 # Divide a circle into segments
if frame_count % 2 == 0 and len(path.points) <= num_slice2:
p = Point()
angle = slice_index2 * 2 * math.pi / num_slice2
slice_index2 += 1
p.x = 4 * math.cos(angle) - 0.5 # Some random circular trajectory, with radius 4, and offset (-0.5, 1, .05)
p.y = 4 * math.sin(angle) + 1.0
p.z = 0.05
rob.pose.position = p
path.points.append(p) # For drawing path, which is line strip type
marker_pub.publish(rob)
marker_pub.publish(path)
"""
# To here, we finished displaying our components
# Check if there is a subscriber. Here our subscriber will be Rviz
while marker_pub.get_num_connections() < 1:
if rospy.is_shutdown():
return 0
# rospy.logwarn_once("Please run Rviz in another terminal.")
rospy.sleep(1)
loop_rate.sleep()
frame_count += 1
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
#try:
# left_pidin, right_pidin = self.arduino.get_pidin()
#except:
# rospy.logerr("getpidout exception count: ")
# return
#self.lEncoderPub.publish(left_pidin)
#self.rEncoderPub.publish(right_pidin)
#try:
# left_pidout, right_pidout = self.arduino.get_pidout()
#except:
# rospy.logerr("getpidout exception count: ")
# return
#self.lPidoutPub.publish(left_pidout)
#self.rPidoutPub.publish(right_pidout)
# Read the encoders
try:
left_enc, right_enc = self.arduino.get_encoder_counts()
rospy.loginfo("left_enc: " + str(left_enc) + "right_enc: " +
str(right_enc))
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
if (left_enc < self.encoder_low_wrap
and self.enc_left > self.encoder_high_wrap):
self.l_wheel_mult = self.l_wheel_mult + 1
elif (left_enc > self.encoder_high_wrap
and self.enc_left < self.encoder_low_wrap):
self.l_wheel_mult = self.l_wheel_mult - 1
else:
self.l_wheel_mult = 0
if (right_enc < self.encoder_low_wrap
and self.enc_right > self.encoder_high_wrap):
self.r_wheel_mult = self.r_wheel_mult + 1
elif (right_enc > self.encoder_high_wrap
and self.enc_right < self.encoder_low_wrap):
self.r_wheel_mult = self.r_wheel_mult - 1
else:
self.r_wheel_mult = 0
#dright = (right_enc - self.enc_right) / self.ticks_per_meter
#dleft = (left_enc - self.enc_left) / self.ticks_per_meter
dleft = 1.0 * (left_enc + self.l_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_left) / self.ticks_per_meter
dright = 1.0 * (right_enc + self.r_wheel_mult *
(self.encoder_max - self.encoder_min) -
self.enc_right) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
# todo sensor_state.distance == 0
#if self.v_des_left == 0 and self.v_des_right == 0:
# odom.pose.covariance = ODOM_POSE_COVARIANCE2
# odom.twist.covariance = ODOM_TWIST_COVARIANCE2
#else:
# odom.pose.covariance = ODOM_POSE_COVARIANCE
# odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def poll(self):
if self.debugPID:
try:
A_pidin, B_pidin, C_pidin = self.arduino.get_pidin()
self.AEncoderPub.publish(A_pidin)
self.BEncoderPub.publish(B_pidin)
self.CEncoderPub.publish(C_pidin)
except:
rospy.logerr("getpidin exception count:")
return
try:
A_pidout, B_pidout, C_pidout = self.arduino.get_pidout()
self.APidoutPub.publish(A_pidout)
self.BPidoutPub.publish(B_pidout)
self.CPidoutPub.publish(C_pidout)
except:
rospy.logerr("getpidout exception count")
return
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts(
)
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
#rospy.loginfo("Encoder A:"+str(aWheel_enc)+",B:"+str(bWheel_enc)+",C:" + str(cWheel_enc))
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_A == None and self.enc_B == None and self.enc_C == None:
d_A = 0
d_B = 0
d_C = 0
else:
d_A = (aWheel_enc - self.enc_A) / self.ticks_per_meter
d_B = (bWheel_enc - self.enc_B) / self.ticks_per_meter
d_C = (cWheel_enc - self.enc_C) / self.ticks_per_meter
self.enc_A = aWheel_enc
self.enc_B = bWheel_enc
self.enc_C = cWheel_enc
va = d_A / dt
vb = d_B / dt
vc = d_C / dt
vx = sqrt(3) * (va - vb) / 3.0
vy = (va + vb - 2 * vc) / 3.0
vth = (va + vb + vc) / (3 * self.wheel_track)
delta_x = (vx * cos(self.th) - vy * sin(self.th)) * dt
delta_y = (vx * sin(self.th) + vy * cos(self.th)) * dt
delta_th = vth * dt
self.x += delta_x
self.y += delta_y
self.th += delta_th
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.pose.covariance = [
1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
]
odom.twist.twist.linear.x = vx
odom.twist.twist.linear.y = vy
odom.twist.twist.angular.z = vth
odom.twist.covariance = [
1e-9, 0, 0, 0, 0, 0, 0, 1e-3, 1e-9, 0, 0, 0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e6, 0, 0, 0, 0, 0, 0, 1e-9
]
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.stopped = True
self.v_des_AWheel = 0
self.v_des_BWheel = 0
self.v_des_CWheel = 0
if self.v_A < self.v_des_AWheel:
self.v_A += self.max_accel
if self.v_A > self.v_des_AWheel:
self.v_A = self.v_des_AWheel
else:
self.v_A -= self.max_accel
if self.v_A < self.v_des_AWheel:
self.v_A = self.v_des_AWheel
if self.v_B < self.v_des_BWheel:
self.v_B += self.max_accel
if self.v_B > self.v_des_BWheel:
self.v_B = self.v_des_BWheel
else:
self.v_B -= self.max_accel
if self.v_B < self.v_des_BWheel:
self.v_B = self.v_des_BWheel
if self.v_C < self.v_des_CWheel:
self.v_C += self.max_accel
if self.v_C > self.v_des_CWheel:
self.v_C = self.v_des_CWheel
else:
self.v_C -= self.max_accel
if self.v_C < self.v_des_CWheel:
self.v_C = self.v_des_CWheel
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_A, self.v_B, self.v_C)
if self.debugPID:
self.AVelPub.publish(self.v_A)
self.BVelPub.publish(self.v_B)
self.CVelPub.publish(self.v_C)
else:
self.stop()
self.t_next = now + self.t_delta
def find_blocks(self):
cv_image = self.cv_image
block_marker_list = MarkerArray()
ray_marker_list = MarkerArray()
inv_block_obs_list = []
ws_block_obs_list = []
block_pixel_locs_list = []
# Enables tuning of HSV thresholds for different lighting conditions
height, width, depth = cv_image.shape
pad_size = 5
if (self.camera == "top"):
# Remove table from hsv image
hsv = remove_table(cv_image)
else:
hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
images = [hsv, hsv, hsv, hsv, hsv]
i = 0
ray_id = 0
# Find table
if (self.camera == "right_hand"):
if (self.ir_reading is not None):
area_min_threshold = 400 / self.ir_reading
area_max_threshold = 100000 # TODO: tune
else:
area_min_threshold = 400 / 0.4
area_max_threshold = 100000 # TODO: tune
elif (self.camera == "top"):
area_min_threshold = 40
area_max_threshold = 1000
for color in colors:
low_h = colors[color]["low_h"]
high_h = colors[color]["high_h"]
low_s = colors[color]["low_s"]
high_s = colors[color]["high_s"]
low_v = colors[color]["low_v"]
high_v = colors[color]["high_v"]
# Converting image to HSV format
if color == "red":
hsv_mask_1 = cv2.inRange(hsv,
np.array([low_h[0], low_s, low_v]),
np.array([high_h[0], high_s, high_v]))
hsv_mask_2 = cv2.inRange(hsv,
np.array([low_h[1], low_s, low_v]),
np.array([high_h[1], high_s, high_v]))
hsv_mask = hsv_mask_1 | hsv_mask_2
else:
hsv_mask = cv2.inRange(hsv, np.array([low_h, low_s, low_v]),
np.array([high_h, high_s, high_v]))
# Apply mask to original image
masked_img = cv2.bitwise_and(cv_image, cv_image, mask=hsv_mask)
# Store HSV masked image for the current color
self.seg_img[color] = masked_img.copy()
# cv2.imshow(color, masked_img)
# Morphological opening (remove small objects from the foreground)
erode_1 = cv2.erode(hsv_mask,
np.ones((5, 5), np.uint8),
iterations=1)
dilate_1 = cv2.dilate(erode_1,
np.ones((5, 5), np.uint8),
iterations=1)
# Morphological closing (fill small holes in the foreground)
dilate_2 = cv2.dilate(dilate_1,
np.ones((10, 10), np.uint8),
iterations=1)
erode_2 = cv2.erode(dilate_2,
np.ones((10, 10), np.uint8),
iterations=1)
images[i] = erode_2.copy()
ret, thresh = cv2.threshold(erode_2, 157, 255, 0)
if (OPENCV3):
im2, contours, hierarchy = cv2.findContours(
thresh, cv2.RETR_TREE, cv2.CHAIN_APPROX_NONE)
else:
contours, hierarchy = cv2.findContours(thresh, cv2.RETR_TREE,
cv2.CHAIN_APPROX_NONE)
# Draw the countours.
# cv2.drawContours(cv_image, contours, -1, (0,255,0), 3)
if (self.camera == "right_hand"):
if (RES_640x400):
# At 0 Meters
cv2.circle(cv_image, (325, 129), 5, (255, 0, 255), 1)
# At -0.15 Meters
cv2.circle(cv_image, (330, 94), 5, (255, 255, 0), 1)
elif (RES_1280x800):
# At 0 Meters
cv2.circle(cv_image, (645, 329), 5, (255, 0, 255), 1)
# At -0.15 Meters
cv2.circle(cv_image, (650, 294), 5, (255, 255, 0), 1)
# At -0.16 Meters
cv2.circle(cv_image, (660, 285), 5, (0, 255, 255), 1)
# rospy.loginfo("Found %d %s objects", num_obj, color)
for contour in contours:
area = cv2.contourArea(contour)
if (area > area_min_threshold and area < area_max_threshold):
x, y, w, h = cv2.boundingRect(contour)
rect = cv2.minAreaRect(contour)
# rospy.loginfo("AREA: %f", area)
if (OPENCV3):
box = cv2.boxPoints(rect)
else:
box = cv2.cv.BoxPoints(rect)
box = np.int0(box)
# Pad the outside of cropped image so that images edges
# can still effect PCA
x_padded_min = x - pad_size
x_padded_max = x + w + pad_size
y_padded_min = y - pad_size
y_padded_max = y + h + pad_size
if x_padded_min < 0:
x_padded_min = 0
if y_padded_min < 0:
y_padded_min = 0
if x_padded_max > width:
x_padded_max = width
if y_padded_max > height:
y_padded_max = height
cropped_img = masked_img[y_padded_min:y_padded_max,
x_padded_min:x_padded_max]
if (self.camera == "top"):
#cropped_img = np.flip(cropped_img, 0)
#block_angle = 0
block_angle = calc_angle(cropped_img) + math.pi / 2
else:
block_angle = calc_angle(cropped_img)
# block_angle = rect[2]
rospy.loginfo("Block angle is %f", block_angle)
# TODO: Is this the same?
# block_angle = rect[2]
rospy.loginfo("Block angle is %f", block_angle)
# block_ratio = calc_ratio(rect[1][1], rect[1][0])
block_length, block_width = calc_block_type(
rect[1][1], rect[1][0], self.camera)
moms = cv2.moments(contour)
if (moms['m00'] > area_min_threshold
and moms['m00'] < area_max_threshold):
cx = int(moms['m10'] / moms['m00'])
cy = int(moms['m01'] / moms['m00'])
if (self.camera == "top"):
# Block should not be outside of circle centered at 319,255 with radius 200
d = math.sqrt((cx - 319)**2 + (cy - 255)**2)
if (d > 198):
continue
# cx = cx - self.camera_model.cx()
# cy = cy - self.camera_model.cy()
print("Color: ", color, " Box: ", box)
cv2.drawContours(cv_image, [box], 0, color_vals[color],
1)
# Outlines for Barbell Task
"""
cv2.rectangle(cv_image, (216 - 5, 251 + 5), (244 + 5,238 - 5), (0,0,0), 1)
cv2.rectangle(cv_image, (224 - 5, 301 + 5), (238 + 5,253 - 5), (0,0,0), 1)
cv2.rectangle(cv_image, (218 - 5, 316 + 5), (244 + 5,303 - 5), (0,0,0), 1)
"""
cv2.circle(cv_image, (cx, cy), 3, color_vals[color], 1)
# Write the block tshape
font = cv2.FONT_HERSHEY_SIMPLEX
if (self.camera == "top"):
font_size = 0.5
font_thickness = 1
else:
font_size = 3.0
font_thickness = 2
cv2.putText(
cv_image,
block_type_string(block_length,
block_width), (cx, cy), font,
font_size, color_vals[color], font_thickness)
self.pixel_loc = [cx, cy]
if (self.camera == "right_hand"):
vec = np.array(
self.hand_camera_model.projectPixelTo3dRay(
(cx, cy)))
elif (self.camera == "top"):
vec = np.array(
self.top_camera_model.projectPixelTo3dRay(
(cx, cy)))
new_vec = vec.copy()
new_vec[0] = vec[2]
new_vec[1] = -vec[0]
new_vec[2] = -vec[1]
vec = new_vec.copy()
if (in_workspace((cx, cy))):
cv2.putText(cv_image, "WS", (cx + 10, cy + 10),
font, font_size, color_vals[color],
font_thickness)
else:
cv2.putText(cv_image, "INV",
(cx + 10, cy + 10), font,
font_size, color_vals[color],
font_thickness)
else:
# Invalid camera name
rospy.loginfo("%s is an invalid camera!",
self.camera)
return
# If we're using the hand camera, make sure we have a valid IR reading...
if (self.camera == "top"
or (self.camera == "right_hand"
and self.ir_reading is not None)):
if (self.camera == "right_hand"):
d = self.ir_reading + 0.15
elif (self.camera == "top"):
d = self.top_cam_table_dist
ray_pt_1 = np.array([0, 0, 0])
ray_pt_2 = 2 * vec
# rospy.loginfo("Vec: %f, %f, %f", vec[0], vec[1], vec[2])
d_cam = d * vec
homog_d_cam = np.concatenate(
(d_cam, np.ones(1))).reshape((4, 1))
homog_ray_pt_1 = np.concatenate(
(ray_pt_1, np.ones(1))).reshape((4, 1))
homog_ray_pt_2 = np.concatenate(
(ray_pt_2, np.ones(1))).reshape((4, 1))
if (self.camera == "top"):
camera_to_base = self.top_to_base_mat
"""
self.tf_listener.waitForTransform(
'/base', self.camera + "_camera", rospy.Time(), rospy.Duration(4))
(trans, rot) = self.tf_listener.lookupTransform(
'/base', self.camera + "_camera", rospy.Time())
"""
else:
# Wait for transformation from base to camera as this change as the hand_camera moves
self.tf_listener.waitForTransform(
'/base', self.camera + "_camera",
rospy.Time(), rospy.Duration(4))
(trans,
rot) = self.tf_listener.lookupTransform(
'/base', self.camera + "_camera",
rospy.Time())
camera_to_base = tf.transformations.compose_matrix(
translate=trans,
angles=tf.transformations.
euler_from_quaternion(rot))
block_position_arr = np.dot(
camera_to_base, homog_d_cam)
# Create a ray from the camera to the detected block
# Transform ray to base frame
ray_pt_1_tf = np.dot(camera_to_base,
homog_ray_pt_1)
ray_pt_2_tf = np.dot(camera_to_base,
homog_ray_pt_2)
ray_marker_list.markers.append(
create_ray_marker(
frame="base",
id=ray_id,
point1=Point(ray_pt_1_tf[0],
ray_pt_1_tf[1],
ray_pt_1_tf[2]),
point2=Point(ray_pt_2_tf[0],
ray_pt_2_tf[1],
ray_pt_2_tf[2]),
ray_color=color,
transparency=self.transparency))
ray_id += 1
# rospy.loginfo("Block position: %f, %f, %f", block_position_arr[0], block_position_arr[1], block_position_arr[2])
# rospy.loginfo("Block angle: %f", math.degrees(block_angle))
block_position_p = Point()
block_position_arr_copy = block_position_arr.copy()
block_position_p.x = block_position_arr_copy[0]
block_position_p.y = block_position_arr_copy[1]
block_position_p.z = -.1
# TODO: double check that the angle is correct (What if camera rotates?)
# Rotation about z-axis
block_orientation_arr = tf.transformations.quaternion_from_euler(
0, 0, block_angle)
block_orientation = Quaternion()
block_orientation.x = block_orientation_arr[0]
block_orientation.y = block_orientation_arr[1]
block_orientation.z = block_orientation_arr[2]
block_orientation.w = block_orientation_arr[3]
"""
if(self.camera == "top"):
block_angle += (1.34 - math.pi/2)
rot_quat = tf.transformations.quaternion_multiply(
rot, block_orientation)
block_angle = tf.transformations.quaternion_from_euler(
rot_quat)
"""
# Create a marker to visualize in RVIZ
curr_marker = create_block_marker(
frame="base",
id=len(block_marker_list.markers),
position=block_position_p,
orientation=block_orientation,
length=block_length,
width=block_width,
block_color=color,
transparency=self.transparency)
rospy.loginfo("Adding new marker")
block_marker_list.markers.append(curr_marker)
#block_pose_list.append(Pose(position=block_position_p, orientation=block_orientation))
# TODO: The block angle will still be wrong. Need to transform it from the camera coordinate to the world frame
if (in_workspace((cx, cy))):
ws_block_obs_list.append(
BlockObservation(pose=Pose2D(
x=block_position_p.x,
y=block_position_p.y,
theta=block_angle),
color=color,
length=block_length,
width=block_width))
else:
inv_block_obs_list.append(
BlockObservation(pose=Pose2D(
x=block_position_p.x,
y=block_position_p.y,
theta=block_angle),
color=color,
length=block_length,
width=block_width))
block_pixel_locs_list.append(
BlockPixelLoc(x=cx,
y=cy,
theta=block_angle,
color=color,
length=block_length,
width=block_width))
else:
# rospy.loginfo("No ir_data has been recieved yet!")
pass
else:
# rospy.loginfo("Moments aren't large enough!")
pass
else:
pass
# rospy.loginfo("Contour area is not large enough!")
self.rect_seg_img = cv_image.copy()
self.ray_markers = ray_marker_list
self.block_markers = block_marker_list
self.inv_block_obs = inv_block_obs_list
self.ws_block_obs = ws_block_obs_list
self.block_pixel_locs = block_pixel_locs_list
self.inv_detected_blocks = len(inv_block_obs_list)
self.ws_detected_blocks = len(ws_block_obs_list)
rospy.Subscriber("simu_send_gps", GPSFix, sub_gps)
pub_send_lines_to_follow = rospy.Publisher('control_send_lines_to_follow',
Quaternion,
queue_size=10)
lines_to_follow_msg = Quaternion()
rate = rospy.Rate(10)
triangle = [[50.695396, -4.236313], [50.696210, -4.235734],
[50.695266,
-4.235562]] #en deuxieme pour lisser [50.696190,-4.236122]
n = len(triangle)
point = 0
while not rospy.is_shutdown():
# 50.695396 -4.236313
# 50.696210 -4.235734
# 50.695266 -4.235562
lines_to_follow_msg.x = triangle[point % n][0]
lines_to_follow_msg.y = triangle[point % n][1]
lines_to_follow_msg.z = triangle[(point + 1) % n][0]
lines_to_follow_msg.w = triangle[(point + 1) % n][1]
pub_send_lines_to_follow.publish(lines_to_follow_msg)
dist = (boat_lat - triangle[(point + 1) % n][0])**2 + (
boat_long - triangle[(point + 1) % n][1])**2
if dist < 10**-9:
point += 1
#rospy.loginfo("[{}] lat:{}, lon:{}".format(node_name,dist,point))
rate.sleep()
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
# Read the encoders
try:
self.diagnostics.reads += 1
self.diagnostics.total_reads += 1
left_enc, right_enc = self.arduino.get_encoder_counts()
self.diagnostics.freq_diag.tick()
except:
self.diagnostics.errors += 1
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# Calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = self.odom_linear_scale_correction * (dright +
dleft) / 2.0
dth = self.odom_angular_scale_correction * (
dright - dleft) / float(self.wheel_track)
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
if self.publish_odom_base_transform:
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
self.current_speed = Twist()
self.current_speed.linear.x = vxy
self.current_speed.angular.z = vth
"""
Covariance values taken from Kobuki node odometry.cpp at:
https://github.com/yujinrobot/kobuki/blob/indigo/kobuki_node/src/library/odometry.cpp
Pose covariance (required by robot_pose_ekf) TODO: publish realistic values
Odometry yaw covariance must be much bigger than the covariance provided
by the imu, as the later takes much better measures
"""
odom.pose.covariance[0] = 0.1
odom.pose.covariance[7] = 0.1
if self.use_imu_heading:
#odom.pose.covariance[35] = 0.2
odom.pose.covariance[35] = 0.05
else:
odom.pose.covariance[35] = 0.05
odom.pose.covariance[
14] = sys.float_info.max # set a non-zero covariance on unused
odom.pose.covariance[
21] = sys.float_info.max # dimensions (z, pitch and roll); this
odom.pose.covariance[
28] = sys.float_info.max # is a requirement of robot_pose_ekf
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if not self.stopped:
self.arduino.drive(self.v_left, self.v_right)
self.t_next = now + self.t_delta
def timer_callback(self):
senses = self.robot.sensors([create.WALL_SIGNAL, create.WALL_IR_SENSOR, create.LEFT_BUMP, create.RIGHT_BUMP, create.ENCODER_LEFT, create.ENCODER_RIGHT, create.CLIFF_LEFT_SIGNAL, create.CLIFF_FRONT_LEFT_SIGNAL, create.CLIFF_FRONT_RIGHT_SIGNAL, create.CLIFF_RIGHT_SIGNAL, create.DIRT_DETECTED])
current_time = self.get_clock().now().to_msg()
# バンパーセンサー
bumper = Bumper()
bumper.header.frame_id = "bumper"
bumper.header.stamp = current_time
bumper.is_left_pressed = senses[create.LEFT_BUMP] == 1
bumper.is_right_pressed = senses[create.RIGHT_BUMP] == 1
bumper.light_signal_left = self.robot.senseFunc(create.LIGHTBUMP_LEFT)()
bumper.light_signal_front_left = self.robot.senseFunc(create.LIGHTBUMP_FRONT_LEFT)()
bumper.light_signal_center_left = self.robot.senseFunc(create.LIGHTBUMP_CENTER_LEFT)()
bumper.light_signal_center_right = self.robot.senseFunc(create.LIGHTBUMP_CENTER_RIGHT)()
bumper.light_signal_front_right = self.robot.senseFunc(create.LIGHTBUMP_FRONT_RIGHT)()
bumper.light_signal_right = self.robot.senseFunc(create.LIGHTBUMP_RIGHT)()
self.bumper_pub_.publish(bumper)
# エンコーダー
enc_left = senses[create.ENCODER_LEFT]
enc_right = senses[create.ENCODER_RIGHT]
# 回転を計算
if self.enc_left_old is not None and self.enc_right_old is not None:
left_diff = self.robot._getEncoderDelta(self.enc_left_old, enc_left)
right_diff = self.robot._getEncoderDelta(self.enc_right_old, enc_right)
left_mm = math.pi * left_diff / create.TICK_PER_MM / 180
right_mm = math.pi * right_diff / create.TICK_PER_MM / 180
print(left_mm, right_mm)
self.enc_left_old = enc_left
self.enc_right_old = enc_right
# オドメトリを取得 [m, m, rad]
x, y, th = self.robot.getPose(dist='cm',angle='deg')
x, y = x/100, y/100
th = math.pi + th / 180
# since all odometry is 6DOF we'll need a quaternion created from yaw
odom_quat = self.euler_to_quaternion(0, 0, th)
# first, we'll publish the transform over tf2_ros
transform_stamped = TransformStamped()
transform_stamped.header.stamp = current_time
transform_stamped.header.frame_id = "base_link"
transform_stamped.child_frame_id = "odom"
transform_stamped.transform.translation.x = x
transform_stamped.transform.translation.y = y
transform_stamped.transform.translation.z = 0.0
transform_stamped.transform.rotation.x = odom_quat[0]
transform_stamped.transform.rotation.y = odom_quat[1]
transform_stamped.transform.rotation.z = odom_quat[2]
transform_stamped.transform.rotation.w = odom_quat[3]
odom_broadcaster = tf2_ros.TransformBroadcaster(self, qos_profile_sensor_data)
odom_broadcaster.sendTransform(transform_stamped)
# next, we'll publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
# set the position
pt = Point()
pt.x = x
pt.y = y
pt.z = 0.0
quat = Quaternion()
quat.x = odom_quat[0]
quat.y = odom_quat[1]
quat.z = odom_quat[2]
quat.w = odom_quat[3]
pose = Pose()
pose.position = pt
pose.orientation = quat
odom.pose.pose = pose
# publish the message
self.odom_pub_.publish(odom)
# 傾きセンサー
senses[create.CLIFF_LEFT_SIGNAL]
senses[create.CLIFF_FRONT_LEFT_SIGNAL]
senses[create.CLIFF_FRONT_RIGHT_SIGNAL]
senses[create.CLIFF_RIGHT_SIGNAL]
def _BroadcastOdometryInfo(self, lineParts):
partsCount = len(lineParts)
# rospy.logwarn(lineParts)
# if (partsCount <= 6):
# pass
try:
# rospy.logwarn("X")
# rospy.logwarn(lineParts[1])
x = float(lineParts[1])
# rospy.logwarn(line_parts_float)
# line_parts_int = round(line_parts_float)
# rospy.logwarn(line_parts_int)
# x = line_parts_int / 1000.0
# rospy.logwarn(x)
# rospy.logwarn("Y")
# rospy.logwarn(lineParts[2])
y = float(lineParts[2])
# line_parts_int = round(line_parts_float)
# y = line_parts_int / 1000.0
# rospy.logwarn(y)
theta = float(lineParts[3])
# line_parts_int = round(line_parts_float)
# theta = line_parts_int / 1000.0
vx = float(lineParts[4])
# line_parts_int = round(line_parts_float)
# vx = line_parts_int / 1000.0
omega = float(lineParts[5])
# rospy.logwarn(line_parts_float)
# line_parts_int = round(line_parts_float)
# omega = line_parts_int / 1000.0
#quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
quaternion = Quaternion()
quaternion.x = 0
quaternion.y = 0
quaternion.z = sin(theta / 2.0)
quaternion.w = cos(theta / 2.0)
rosNow = rospy.Time.now()
# First, we'll publish the transform from frame odom to frame base_link over tf
# Note that sendTransform requires that 'to' is passed in before 'from' while
# the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
self._OdometryTransformBroadcaster.sendTransform(
(x, y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rosNow, "base_footprint", "odom")
# next, we'll publish the odometry message over ROS
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = rosNow
odometry.pose.pose.position.x = x
odometry.pose.pose.position.y = y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = vx
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = omega
self._OdometryPublisher.publish(odometry)
except:
# rospy.logwarn("Unexpected error:" + str(sys.exc_info()[0]))
rospy.logwarn("Error from odometry pub")
def reset(self,
testxml=0,
xml_human_x=-1,
xml_human_y=-1,
xml_human_rotation_z=-1,
xml_robot_x=-1,
xml_robot_y=-1,
xml_robot_rotation_z=-1):
if testxml == 0:
for random_iter in range(20):
if self.my_case == -1:
print("hjktest---- reset this", random_iter)
randomrobotINT = random.randint(0, 9)
if randomrobotINT == 0:
robotx = 1.5
roboty = 0.1
robotz = 1.5
elif randomrobotINT == 1:
robotx = 2
roboty = 0.1
robotz = 1.5
elif randomrobotINT == 2:
robotx = 3
roboty = 0.1
robotz = 1.5
else:
robotx = random.random() * (5.5 - 2) + 2
roboty = 0.1
robotz = random.random() * (8 - 4) + 4
#robotx = random.random() * (5 - 1.5) + 1.5
#roboty = 0.1
#robotz = random.random() * (8 - 1.5) + 1.5
'''
robotx = random.random() * (5.5 - 3.8) + 3.8
roboty = 0.1
robotz = random.random() * (4.5 - 2.7) + 2.7
'''
robot_rotat_x = 0
robot_rotat_y = 1
robot_rotat_z = 0
robot_rotat_w = random.uniform(-PI,
PI) #-3.14, 3.14) #-PI*4/5
randomINT = random.randint(0, 4)
if randomINT == 0:
humanx = 3
humany = 1.27
humanz = 6
elif randomINT == 1:
humanx = 4.5
humany = 1.27
humanz = 7.5
elif randomINT >= 2 and randomINT <= 4:
humanx = 2.5
humany = 1.27
humanz = 2
else:
humanx = random.random() * (4 - 2.5) + 2.5
humany = 1.27
humanz = random.random() * (7.5 - 2.5) + 2.5
else:
robotx = my_case_robotx[self.my_case]
roboty = my_case_roboty[self.my_case]
robotz = my_case_robotz[self.my_case]
robot_rotat_x = my_case_robot_rotat_x[self.my_case]
robot_rotat_y = my_case_robot_rotat_y[self.my_case]
robot_rotat_z = my_case_robot_rotat_z[self.my_case]
robot_rotat_w = my_case_robot_rotat_w[self.my_case]
humanx = my_case_humanx[self.my_case]
humany = my_case_humany[self.my_case]
humanz = my_case_humanz[self.my_case]
###important!!!!!
global global_rn
global_rn = random.random()
### room
position = Vector3()
position.x = robotx
position.y = roboty
position.z = robotz
rotation = Quaternion()
rotation.x = robot_rotat_x
#rotation.y = random.uniform(-3.14, 3.14)
rotation.y = robot_rotat_y
rotation.z = robot_rotat_z
rotation.w = robot_rotat_w
human_pose = Vector3()
human_pose.x = humanx
human_pose.y = humany
human_pose.z = humanz
rpos, rrot = getopposite(human_pose, human.rotation,
self.my_case)
dist1 = getDisXZ(position.x, position.z, rotation.w, rpos.x,
rpos.z, rrot.w)
distRtoH = getDisXZ(position.x, position.z, rotation.w,
human_pose.x, human_pose.z, 0)
if self.my_case != -1 or (dist1 < DISFROMSTARTTOEND
and distRtoH > MINDISFROMSTARTTOEND):
break
elif testxml == 1:
position = Vector3()
position.x = xml_robot_x
position.y = 0.1
position.z = xml_robot_y
rotation = Quaternion()
rotation.x = 0
# rotation.y = random.uniform(-3.14, 3.14)
rotation.y = 1
rotation.z = 0
rotation.w = xml_robot_rotation_z
human_pose = Vector3()
human_pose.x = xml_human_x
human_pose.y = 1.27
human_pose.z = xml_human_y
global global_rn
if xml_human_rotation_z == 1:
global_rn = 0
elif xml_human_rotation_z == 2:
global_rn = 0.6
elif xml_human_rotation_z == 3:
global_rn = 0.2
elif xml_human_rotation_z == 4:
global_rn = 0.9
done = motor.set_velocity(0, 0, 0, 0)
while done is False:
time_step.time_step_call()
done = motor.set_velocity(0, 0, 0, 0)
pub_reset_node_physics_req()
while robot_global.reset_node_physics_res_flag is False:
time_step.time_step_call()
pub_set_position_req(position)
while robot_global.set_position_res_flag is False:
time_step.time_step_call()
pub_set_rotation_req(rotation)
while robot_global.set_rotation_res_flag is False:
time_step.time_step_call()
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
#print('hjk--- not set random :', human.position.x, human.position.y, human.position.z, human.rotation.x, human.rotation.y, human.rotation.z, human.rotation.w)
pub_set_human_pose_req(human_pose)
print("hjk--lll :", human.set_human_pose_res_flag)
while human.set_human_pose_res_flag is False:
time_step.time_step_call()
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
print('hjk--- have set random :', human.position.x, human.position.y,
human.position.z)
laser_data, done = laser.get_laser_scan_data()
while done is False:
laser_data, done = laser.get_laser_scan_data()
time_step.time_step_call()
laser_state, is_collision = discretize_observation(
laser_data, self.laser_dim, self.collision_threshold)
for i in range(0, 10):
time_step.time_step_call()
###############
self.action_history1 = 0
self.action_history2 = 0
self.action_history3 = 0
#get human pose
pub_get_human_position_req()
while human.get_position_res_flag is False:
time_step.time_step_call()
pub_get_human_rotation_req()
while human.get_rotation_res_flag is False:
time_step.time_step_call()
# self.mp = np.zeros((self.N,self.M))
rpos, rrot = getopposite(human.position, human.rotation, self.my_case)
dist1 = getDisXZ(robot_global.position.x, robot_global.position.z,
robot_global.rotation.w, rpos.x, rpos.z, rrot.w)
rtang, rot1, rot2 = getanglefea(position.x, position.z, rotation.w,
rpos.x, rpos.z, rrot.w)
print("11111 state dis : ", dist1 * 10)
state = changeStateFromEnvToNetwork(laser_state, dist1, rot1, rot2, -1)
self.distp = dist1
self.lenp = dist1
self.rot1p = rot1
self.rot2p = rot2
# self.old_robot = robot_global
self.old_robot_postion = copy.deepcopy(robot_global.position)
self.old_robot_rotation = copy.deepcopy(robot_global.rotation)
self.init_dist_pos = int(self.distp / 0.5)
#self.init_dist_pos = self.init_dist / 0.5
if self.init_dist_pos > self.wintimes_dist_key:
self.init_dist_pos = self.wintimes_dist_key
print("wwwww????? : ", self.init_dist_pos, self.distp)
self.wintimes_all[
self.init_dist_pos] = self.wintimes_all[self.init_dist_pos] + 1
return np.asarray(state)
def poll(self):
now = rospy.Time.now()
if now > self.t_next:
#modify by william
if self.debugPID:
rospy.logdebug("poll start-------------------------------: ")
try:
if self.four_wd:
left_pidin, right_pidin, left_h_pidin, right_h_pidin = self.arduino.get_pidin(
)
rospy.logdebug("left_pidin: " + str(left_pidin))
rospy.logdebug("right_pidin: " + str(right_pidin))
rospy.logdebug("left_h_pidin: " + str(left_h_pidin))
rospy.logdebug("right_h_pidin: " + str(right_h_pidin))
else:
left_pidin, right_pidin = self.arduino.get_pidin()
rospy.logdebug("left_pidin: " + str(left_pidin))
rospy.logdebug("right_pidin: " + str(right_pidin))
except:
rospy.logerr("getpidin exception count: ")
return
self.lEncoderPub.publish(left_pidin)
self.rEncoderPub.publish(right_pidin)
try:
if self.four_wd:
left_pidout, right_pidout, left_h_pidout, right_h_pidout = self.arduino.get_pidout(
)
rospy.logdebug("left_pidout: " + str(left_pidout))
rospy.logdebug("right_pidout: " + str(right_pidout))
rospy.logdebug("left_h_pidout: " + str(left_h_pidout))
rospy.logdebug("right_h_pidout: " +
str(right_h_pidout))
else:
left_pidout, right_pidout = self.arduino.get_pidout()
rospy.logdebug("left_pidout: " + str(left_pidout))
rospy.logdebug("right_pidout: " + str(right_pidout))
except:
rospy.logerr("getpidout exception count: ")
return
self.lPidoutPub.publish(left_pidout)
self.rPidoutPub.publish(right_pidout)
# Read the encoders
try:
if self.four_wd:
left_enc, right_enc, left_h_enc, right_h_enc = self.arduino.get_encoder_counts(
)
if self.debugPID:
rospy.logdebug("left_enc: " + str(left_enc))
rospy.logdebug("right_enc: " + str(right_enc))
rospy.logdebug("left_h_enc: " + str(left_h_enc))
rospy.logdebug("right_h_enc: " + str(right_h_enc))
else:
left_enc, right_enc = self.arduino.get_encoder_counts()
if self.debugPID:
rospy.logdebug("left_enc: " + str(left_enc))
rospy.logdebug("right_enc: " + str(right_enc))
except:
self.bad_encoder_count += 1
rospy.logerr("Encoder exception count: " +
str(self.bad_encoder_count))
return
dt = now - self.then
self.then = now
dt = dt.to_sec()
# calculate odometry
if self.enc_left == None:
dright = 0
dleft = 0
else:
dright = (right_enc - self.enc_right) / self.ticks_per_meter
dleft = (left_enc - self.enc_left) / self.ticks_per_meter
self.enc_right = right_enc
self.enc_left = left_enc
dxy_ave = (dright + dleft) / 2.0
dth = (dright - dleft) / self.wheel_track
vxy = dxy_ave / dt
vth = dth / dt
if (dxy_ave != 0):
dx = cos(dth) * dxy_ave
dy = -sin(dth) * dxy_ave
self.x += (cos(self.th) * dx - sin(self.th) * dy)
self.y += (sin(self.th) * dx + cos(self.th) * dy)
if (dth != 0):
self.th += dth
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.th / 2.0)
quaternion.w = cos(self.th / 2.0)
# Create the odometry transform frame broadcaster.
self.odomBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
rospy.Time.now(), self.base_frame, "odom")
odom = Odometry()
odom.header.frame_id = "odom"
odom.child_frame_id = self.base_frame
odom.header.stamp = now
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0
odom.pose.pose.orientation = quaternion
odom.twist.twist.linear.x = vxy
odom.twist.twist.linear.y = 0
odom.twist.twist.angular.z = vth
# Create the odometry covariance. robot_pose_ekf needed by sunMaxwell
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE
self.odomPub.publish(odom)
if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
self.v_des_left = 0
self.v_des_right = 0
if self.v_left < self.v_des_left:
self.v_left += self.max_accel
if self.v_left > self.v_des_left:
self.v_left = self.v_des_left
else:
self.v_left -= self.max_accel
if self.v_left < self.v_des_left:
self.v_left = self.v_des_left
if self.v_right < self.v_des_right:
self.v_right += self.max_accel
if self.v_right > self.v_des_right:
self.v_right = self.v_des_right
else:
self.v_right -= self.max_accel
if self.v_right < self.v_des_right:
self.v_right = self.v_des_right
# Set motor speeds in encoder ticks per PID loop
if self.debugPID:
self.lVelPub.publish(self.v_left)
self.rVelPub.publish(self.v_right)
if not self.stopped:
#modify by william
self.arduino.drive(self.v_left, self.v_right)
if self.debugPID:
rospy.logdebug("v_left: " + str(self.v_left))
rospy.logdebug("v_right: " + str(self.v_right))
self.t_next = now + self.t_delta
def hardcoded_Wall():
global global_pos
print('starting node')
rospy.Subscriber('/bebop/odom', Odometry, writeOdom)
rospy.init_node('moveto_tester', anonymous=True, log_level=rospy.WARN)
command = Quaternion()
time.sleep(1)
print(
'Taking off'
) #i think the node was too speedy or something, ignore this takeoff mess its just a hack for testing
pub_takeoff.publish()
# print('huh?')
# time.sleep(5.)
# print('Taking off try 2')
# pub_takeoff.publish()
# print('huh?')
# time.sleep(5.)
time.sleep(1.5)
#### Move 1: Line up with Wall
zcmd = 1.7 - global_pos.position.z # meters, global to body
command.x = 0
command.y = -0.2 # 0.2 meters right
command.z = zcmd
command.w = 0 # Latching disabled
# SEND IT
print('sending command 1: ', command)
pub_commands.publish(command)
time.sleep(5)
#### THIS IS FOR INITIAL TESTING
# pub_land.publish()
#### Move 2, Cross the Wall, go fwd
command.x = 2
command.y = 0
command.z = 0
command.w = 1 # Latching enabled
# SEND IT
print('sending command 2 CROSS THE WALL: ', command)
pub_commands.publish(command)
# wait for it
time.sleep(8)
command.x = 0
command.y = 0 # 0.2 meters right
command.z = 0
command.w = 0 # Latching disabled
# SEND IT
print('sending command 000: ', command)
pub_commands.publish(command)
#### Move 3, Yaw right
command.x = 0
command.y = 0
command.z = 0
command.w = -30 # Yaw Command, positive left
# SEND IT
print('sending command 3, Yaw towards the window: ', command)
pub_commands.publish(command)
# wait for it
time.sleep(4)
#### Move 4, Move initially to center on the window (no latching, let the window controller take over)
command.x = 0
command.y = 0
command.z = 0
command.w = 0 # no latching
# SEND IT
print('sending command 4, Initial centering: ', command)
pub_commands.publish(command)
# wait for it
time.sleep(5)
pub_land.publish()
'/simulation_get_rotation_res', Quaternion, get_rotation_res_callback)
motor.init()
motor.set_velocity(0, 0, 0, 0)
for i in range(0, 3):
time_step.time_step_call()
while True:
position = Vector3()
position.x = random.uniform(-2, 2)
position.y = 0.1
position.z = random.uniform(-2, 2)
rotation = Quaternion()
rotation.x = 0
rotation.y = random.uniform(-3.14, 3.14)
rotation.z = 0
rotation.w = 1
############
done = motor.set_velocity(5, 5, 5, 5)
while done is False:
time_step.time_step_call()
done = motor.set_velocity(5, 5, 5, 5)
for i in range(0, 10):
time_step.time_step_call()
done = motor.set_velocity(0, 0, 0, 0)
while done is False:
time_step.time_step_call()
