def receive_marker_bundle(self, data):
box_tag_seen = False
belt_tag_seen = False
for marker in data.markers:
#just to be sure
if marker.id == TagIds.MasterTag:
box_tag_seen = True
q = utils.msg_to_quaternion(marker.pose.pose.orientation)
roll,pitch,yaw = euler_from_quaternion(q)
self.tags_buffer_box.append(TagAR(marker.id,marker.pose.pose.position.x,
marker.pose.pose.position.y,
marker.pose.pose.position.z,
roll, pitch, yaw))
elif marker.id == TagIds.BeltFakeTag:
belt_tag_seen = True
q = utils.msg_to_quaternion(marker.pose.pose.orientation)
roll,pitch,yaw = euler_from_quaternion(q)
self.tags_buffer_belt.append(TagAR(marker.id,marker.pose.pose.position.x,
marker.pose.pose.position.y,
marker.pose.pose.position.z,
roll, pitch, yaw))
if not box_tag_seen: self.tags_buffer_box.append(None)
if not belt_tag_seen: self.tags_buffer_belt.append(None)
def getTwist(self):
twist = super(MoveTo, self).getTwist()
if twist == None:
twist = Twist()
targetPose = self._getTargetPose()
c_pos = self.pose
d_x = targetPose.position.x - c_pos.translation.x
d_y = targetPose.position.y - c_pos.translation.y
d_len = math.sqrt(d_x ** 2 + d_y ** 2)
##tft.euler_from_quaternion
if d_len < self.target_tresh:
print("close enough")
return None
t_q = targetPose.orientation
c_q = c_pos.rotation
t_euler = tft.euler_from_quaternion((t_q.x, t_q.y, t_q.z, t_q.w))
c_euler = tft.euler_from_quaternion((c_q.x, c_q.y, c_q.z, c_q.w))
d_rot = t_euler[2] - c_euler[2]
# if d_rot > self.slow : #FIXME
# print("small rotation")
# return twist
# if (abs(d_x)/abs(d_y)) < 1:
# print("small rotation")
# print(d_x)
# print(d_y)
# return twist
twist.linear.x = self.speed # TODO add acceleration
# if twist == None && self.target:
# if abs(twist.angular.z
return twist
def listen_imu(self, imu):
# node can sleep for 16 out of 20 ms without dropping any messages
# rospy.sleep(16.0/1000.0)
# self.NN += 1
# if rospy.get_time() - self.t0 > 1:
# rospy.logfatal("Published {} messages in 1 s, {} Hz".format(self.NN, self.NN))
# self.NN, self.t0 = 0, rospy.get_time()
current_orientation = np.array([imu.orientation.x,
imu.orientation.y,
imu.orientation.z,
imu.orientation.w])
current_angular_speed = np.array([imu.angular_velocity.x,
imu.angular_velocity.y,
0.0])
current_rpy = list(transformations.euler_from_quaternion(current_orientation))
# No care about yaw, but must be close to zero
current_rpy[-1] = 0
desired_rpy = list(transformations.euler_from_quaternion(self.desired_orientation))
desired_rpy[-1] = 0
corrected = self.rp_pid(desired_rpy, current_rpy, current_angular_speed)
corrected_orientation = transformations.quaternion_multiply(
transformations.quaternion_from_euler(*corrected),
transformations.quaternion_from_euler(*current_rpy))
setpoint_pose = Pose(self.desired_position, corrected_orientation)
servo_angles = self.platform.ik(setpoint_pose)
rospy.logdebug(
"Servo angles (deg): {}".format(np.rad2deg(servo_angles)))
self.publish_servo_angles(servo_angles)
def callback(data):
# print "Received odom..."
global mapOffset
global odomPub
#make odometry offset from the original odometry by the mapOffset
Odom = Odometry()
Odom.pose.pose.position.x = data.pose.pose.position.x + mapOffset.translation.x
Odom.pose.pose.position.y = data.pose.pose.position.y + mapOffset.translation.y
Odom.pose.pose.position.z = data.pose.pose.position.z + mapOffset.translation.z
# print "O'("+str(Odom.pose.pose.position.x)+","+str(Odom.pose.pose.position.y)+")"
# if (Odom.pose.pose.position.x <0 or Odom.pose.pose.position.y <0):
# print Odom.pose.pose.position
# raise Exception("NEGATIVE COORDINATES! ")
#set the orientation based on the quaternion from the transformed position
quat = data.pose.pose.orientation
q = [quat.x,quat.y,quat.z,quat.w]
roll, pitch, yaw = euler_from_quaternion(q)
quat = mapOffset.rotation
q = [quat.x,quat.y,quat.z,quat.w]
rollOff, pitchOff, yawOff = euler_from_quaternion(q)
roll += rollOff
pitch += pitchOff
yaw += yawOff
quater = quaternion_from_euler(roll, pitch, yaw)
Odom.pose.pose.orientation.w = quater[3]
Odom.pose.pose.orientation.x = quater[0]
Odom.pose.pose.orientation.y = quater[1]
Odom.pose.pose.orientation.z = quater[2]
# print "Publishing shifted odom...\n"
odomPub.publish(Odom)
def correct_orientation(self, target_quat):
current_tf = None
try:
current_tf = self.__tf_listener.lookupTransform('/map', '/base_footprint', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logerr("rotate_in_place_recovery: Failed to look up transform")
return 'failed'
euler_current = euler_from_quaternion(current_tf[1])
euler_target = euler_from_quaternion(target_quat)
rospy.loginfo("Euler Current: %s", str(euler_current))
rospy.loginfo("Euler Target: %s", str(euler_target))
yaw_correction = euler_target[2] - (euler_current[2] + 2*math.pi)
if yaw_correction > math.pi:
yaw_correction -= 2*math.pi
elif yaw_correction < -math.pi:
yaw_correction += 2*math.pi
rospy.loginfo("YAW Correction: %f", yaw_correction)
correction = Twist()
speed = max(abs(yaw_correction) * 0.2, 0.1)
if yaw_correction < 0:
correction.angular.z = -speed
else:
correction.angular.z = speed
self.__cmd_vel.publish(correction)
rospy.sleep(abs(yaw_correction)/speed)
correction.angular.z = 0.0
self.__cmd_vel.publish(correction)
def odomCallback(data):
global STATE
if STATE == 'waiting' or STATE == 'done':
return
elif STATE == 'faceUp':
# Get current yaw in radians
euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w])
yaw = euler[2]
# Check if current yaw is up
if ( abs(yaw-0.0) <= ANGULAR_THRESHOLD ):
stop()
STATE = 'turn'
elif STATE == 'turn':
# Get current yaw in radians
euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w])
yaw = euler[2]
# Check if in desired state
if ( abs(yaw-desiredAngle) <= ANGULAR_THRESHOLD ):
stop()
STATE = 'goToX'
elif STATE == 'goToX':
xpos = #Get x pose.pose.position
if ( abs(xpos-desiredXLocation) <= LINEAR_THRESHOLD ):
stop()
STATE = 'done'
def combinePoses(ps0, ps1, op=operator.add, listener=None):
"""
Returns a PoseStamped of op(ps0,ps1)
"""
# must be in same reference frame
if listener != None:
try:
ps0, ps1 = convertToSameFrameAndTime(ps0, ps1, listener)
except tf.Exception:
return PoseStamped()
if ps0 == None or ps1 == None:
return False
xtrans0, ytrans0, ztrans0 = ps0.pose.position.x, ps0.pose.position.y, ps0.pose.position.z
xtrans1, ytrans1, ztrans1 = ps1.pose.position.x, ps1.pose.position.y, ps1.pose.position.z
wrot0, xrot0, yrot0, zrot0 = ps0.pose.orientation.w, ps0.pose.orientation.x, ps0.pose.orientation.y, ps0.pose.orientation.z
wrot1, xrot1, yrot1, zrot1 = ps1.pose.orientation.w, ps1.pose.orientation.x, ps1.pose.orientation.y, ps1.pose.orientation.z
ps0rot0, ps0rot1, ps0rot2 = tft.euler_from_quaternion([xrot0, yrot0, zrot0, wrot0])
ps1rot0, ps1rot1, ps1rot2 = tft.euler_from_quaternion([xrot1, yrot1, zrot1, wrot1])
addedPoint = Point(op(xtrans0,xtrans1), op(ytrans0,ytrans1), op(ztrans0,ztrans1))
addedEuler = [op(ps0rot0,ps1rot0), op(ps0rot1,ps1rot1), op(ps0rot2,ps1rot2)]
addedQuaternion = tft.quaternion_from_euler(addedEuler[0], addedEuler[1], addedEuler[2])
addedOrientation = Quaternion(addedQuaternion[0], addedQuaternion[1], addedQuaternion[2], addedQuaternion[3])
addedPose = PoseStamped()
addedPose.header = ps0.header
addedPose.pose.position = addedPoint
addedPose.pose.orientation = addedOrientation
return addedPose
def init_plan(self, req):
"""
add in vect_mark[0] the coordinates of the camera deduced from
the position of the mark we choose to be the origin. This step
enable the trackin in our plan.
"""
try:
marker = MARKER_NAME + str(req.marknumber)
trans, rot = self.listener.lookupTransform(
marker, '/map', rospy.Time(0))
while euler_from_quaternion(rot)[0] > 1.57:
time.sleep(0.01)
print "SLEEEEEEEEEEp"
trans, rot = self.listener.lookupTransform(
marker, '/map', rospy.Time(0))
self.vect_tf[0] = [CAMERA_NAME, MAP, trans, rot]
print " plan intialization...!"
print "euler", euler_from_quaternion(self.vect_tf[0][3])
if req.permanent == True:
chdir(req.path)
mon_fichier = open(FILE_SAVED_INIT_PLAN, "w")
message = write_message([self.vect_tf[0]])
# print "message", message
mon_fichier.write(message)
mon_fichier.close()
print self.vect_tf
return InitPlanResponse(True)
except Exception, exc:
print "init_plan"
def callback(self, msg):
"""
Measuring the displacement based on the new position
:param msg: Odometry msg with position and rotation information
:type msg: nag_msgs.msg.Odometry
:return: no return
"""
if self.last_pose:
new_pose = msg.pose.pose
pos = new_pose.position
pos_old = self.last_pose.position
distance_delta = sqrt(pow(pos.x-pos_old.x, 2) + pow(pos.y-pos_old.y, 2))
if distance_delta < 0.5: # If delta is too big, it is incorrect. Not doing anything with this data
self.total_distance += distance_delta
new_orientation = new_pose.orientation
old_orientation = self.last_pose.orientation
new_rotation = euler_from_quaternion([new_orientation.x,
new_orientation.y,
new_orientation.z,
new_orientation.w])
old_rotation = euler_from_quaternion([old_orientation.x,
old_orientation.y,
old_orientation.z,
old_orientation.w])
rotation_delta = new_rotation[2] - old_rotation[2]
if rotation_delta >= pi:
rotation_delta -= 2*pi
elif rotation_delta <= -pi:
rotation_delta += 2*pi
self.total_rotation += abs(rotation_delta)
else:
rospy.logerr("Distance delta too big (%f m), ignoring this step" % distance_delta)
self.last_pose = msg.pose.pose
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 run1(data):
global pointData, points, feedbackData
if data.time == 0:
pointData = data
elif abs(data.time) < 5:
it = abs(data.time)
data.x = feedbackData.feedback.base_position.pose.position.x
data.y = feedbackData.feedback.base_position.pose.position.y
data.z = data.time / abs(data.time)
quaternion = (feedbackData.feedback.base_position.pose.orientation.x, feedbackData.feedback.base_position.pose.orientation.y, feedbackData.feedback.base_position.pose.orientation.z, feedbackData.feedback.base_position.pose.orientation.w)
z = euler_from_quaternion(quaternion, axes='sxyz')
data.z = z[2] + data.z * math.pi / 2
points[it] = data
rospy.loginfo("Add %d is : X:%f Y:%f Theta:%f" % (it, data.x, data.y, data.z))
elif data.time == 5:
data.x = feedbackData.feedback.base_position.pose.position.x
data.y = feedbackData.feedback.base_position.pose.position.y
data.z = data.time / abs(data.time)
quaternion = (feedbackData.feedback.base_position.pose.orientation.x, feedbackData.feedback.base_position.pose.orientation.y, feedbackData.feedback.base_position.pose.orientation.z, feedbackData.feedback.base_position.pose.orientation.w)
z = euler_from_quaternion(quaternion, axes='sxyz')
data.z = z[2]
points[0] = data
rospy.loginfo("Add home is : X:%f Y:%f Theta:%f" % (data.x, data.y, data.z))
elif points[data.time-5] is not None:
pointData.x = points[data.time-5].x
pointData.y = points[data.time-5].y
pointData.z = points[data.time-5].z
pointData.time = data.time
else:
rospy.loginfo("I can't find it.")
def callback(truth,odom):
# Calculate error between truth and SLAM estimate
x_ODOM_error = truth.pose.pose.position.x - odom.pose.pose.position.x
y_ODOM_error = truth.pose.pose.position.y - odom.pose.pose.position.y
xo = odom.pose.pose.orientation.x
yo = odom.pose.pose.orientation.y
zo = odom.pose.pose.orientation.z
wo = odom.pose.pose.orientation.w
xt = truth.pose.pose.orientation.x
yt = truth.pose.pose.orientation.y
zt = truth.pose.pose.orientation.z
wt = truth.pose.pose.orientation.w
# find orientation of robot (Euler coordinates)
(ro, po, yo) = euler_from_quaternion([xo, yo, zo, wo])
(rt, pt, yt) = euler_from_quaternion([xt, yt, zt, wt])
t_ODOM_error = yt-yo
# query SLAM solution
map_listener = tf.TransformListener()
try:
(trans,rot) = map_listener.lookupTransform('/map', '/odom', rospy.Time(0))
x_SLAM_error = trans[0]-x_ODOM_error
y_SLAM_error = trans[1]-y_ODOM_error
t_SLAM_error = rot[2]-t_ODOM_error
print "{0},{1},{2},{3}".format(rospy.get_time(), x_SLAM_error, y_SLAM_error, t_SLAM_error)
except(tf.LookupException, tf.ConnectivityException):
pass
def execute(self, userdata):
rospy.loginfo("Creating goal to put robot in front of handle")
pose_handle = Pose()
if userdata.door_detection_data_in_base_link.handle_side == "left" or userdata.door_detection_data_in_base_link.handle_side == "right": # closed door
(r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_handle.pose.orientation.x,
userdata.door_detection_data_in_base_link.door_handle.pose.orientation.y,
userdata.door_detection_data_in_base_link.door_handle.pose.orientation.z,
userdata.door_detection_data_in_base_link.door_handle.pose.orientation.w)) # gives back r, p, y
theta += 3.1416 # the orientation of the door is looking towards the robot, we need the inverse
pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta)) # orientation to look parallel to the door
pose_handle.position.x = userdata.door_detection_data_in_base_link.door_handle.pose.position.x - 0.4 # to align the shoulder with the handle
pose_handle.position.y = userdata.door_detection_data_in_base_link.door_handle.pose.position.y + 0.2 # refer to upper comment
pose_handle.position.z = 0.0 # we dont need the Z for moving
userdata.door_handle_pose_goal = pose_handle
else: # open door
# if it's open... just cross it?
(r, p, theta) = euler_from_quaternion(( userdata.door_detection_data_in_base_link.door_position.pose.orientation.x,
userdata.door_detection_data_in_base_link.door_position.pose.orientation.y,
userdata.door_detection_data_in_base_link.door_position.pose.orientation.z,
userdata.door_detection_data_in_base_link.door_position.pose.orientation.w)) # gives back r, p, y
theta += 3.1416 # the orientation of the door is looking towards the robot, we need the inverse
pose_handle.orientation = Quaternion(*quaternion_from_euler(0, 0, theta)) # orientation to look parallel to the door
pose_handle.position.x = userdata.door_detection_data_in_base_link.door_position.pose.position.x + 1.0 # enter the door
pose_handle.position.y = userdata.door_detection_data_in_base_link.door_position.pose.position.y
userdata.door_handle_pose_goal = pose_handle
rospy.loginfo("Move base goal: \n" + str(pose_handle))
return succeeded
def ocall(self, data):
twist = Twist()
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = 0
twist.linear.x = 0
twist.linear.y = 0
twist.linear.z = 0
# print data
if self.state == "Turn":
self.odom = data
self.old_euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z,data.pose.pose.orientation.w])
self.state = "Turning"
# delta = data.pose.pose.orientation.z-self.odom.pose.pose.orientation.z
euler = euler_from_quaternion([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z,data.pose.pose.orientation.w])
delta = euler[2]-self.old_euler[2]
if abs(delta) > 3.14:
delta = abs(delta)-6.28
if self.state == "Turning":
if abs(self.Z) >= .3:
self.Z = self.Z*((1.785-abs(delta))/1.785)
twist.angular.z = self.Z
self.pub.publish(twist)
# print euler[2], self.old_euler[2]
print euler[2], delta
if self.state == "Turning" and abs(delta) >= 1.5:
self.X = self.Xo
self.Z = self.Zo
self.pub2.publish('Done')
self.pub.publish(twist)
self.callback("test none")
def updateImu(self, imu):
self.imu_last_update = rospy.Time.now()
self.imu_init = True
if not self.imu_data :
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = self.imu_tf.M * imu_data
angle = imu_data.GetRPY()
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
self.imu_data = True
else:
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = self.imu_tf.M * imu_data
angle = imu_data.GetRPY()
angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
self.odom.pose.pose.orientation.x = angle_quaternion[0]
self.odom.pose.pose.orientation.y = angle_quaternion[1]
self.odom.pose.pose.orientation.z = angle_quaternion[2]
self.odom.pose.pose.orientation.w = angle_quaternion[3]
# Derive angular velocities from orientations #######################
period = (imu.header.stamp - self.last_imu_update).to_sec()
self.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - self.last_imu_orientation[0]) / period
self.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - self.last_imu_orientation[1]) / period
self.odom.twist.twist.angular.z = cola2_lib.normalizeAngle(angle[2] - self.last_imu_orientation[2]) / period
self.last_imu_orientation = angle
self.last_imu_update = imu.header.stamp
#####################################################################
if self.makePrediction():
self.ekf.updatePrediction()
self.publishData()
def tf_trigger(self, reference_frame, target_frame, tfs):
# this function is responsible for setting up the triggers for recording
# on the bagfile.
# sequence for calculating distance and yaw rotation for defining if a
# recording trigger is set according to the trigger value on the yaml file
self.tfL.waitForTransform(reference_frame, target_frame, rospy.Time(0), rospy.Duration(3.0))
trans, rot = self.tfL.lookupTransform(reference_frame, target_frame, rospy.Time(0))
x = trans[0] - self.current_translation[target_frame][0]
y = trans[1] - self.current_translation[target_frame][1]
distance_trans = math.sqrt(x*x + y*y)
distance_rot = abs(euler_from_quaternion(rot)[2] - euler_from_quaternion(self.current_rotation[target_frame])[2])
if("trigger_record_translation" in tfs and distance_trans >= tfs["trigger_record_translation"]):
rospy.loginfo("triggered for translation, trans = " + str(distance_trans))
self.current_translation[target_frame] = trans
self.current_rotation[target_frame] = rot
return "triggered"
if("trigger_record_rotation" in tfs and distance_rot >= tfs["trigger_record_rotation"]):
rospy.loginfo("triggered for rotation, rot = " + str(distance_rot))
self.current_translation[target_frame] = trans
self.current_rotation[target_frame] = rot
return "triggered"
return "not_triggered"
def turn_around():
global turning
global pos
global currz
command = Twist()
send_command = rospy.ServiceProxy('constant_command', ConstantCommand)
w = pos.pose.pose.orientation.w
z = pos.pose.pose.orientation.z
x_theta_new,y_theta_new,z_theta_new = euler_from_quaternion([0,0,z,w])
target = get_new_orientation()
currOrientation = z_theta_new
if(currOrientation + target < 0):
zgoal = 3
else:
zgoal = -3
while(z_theta_new < (target - .1) or z_theta_new > (target + .1)):
if(command.angular.z != zgoal):
command.angular.z = zgoal
send_command(command)
w = pos.pose.pose.orientation.w
z = pos.pose.pose.orientation.z
x_theta_new,y_theta_new,z_theta_new = euler_from_quaternion([0,0,z,w])
command.angular.z = 0
send_command(command)
rospy.sleep(.1)
command.linear.x = .3
send_command(command)
currz = pos.pose.pose.position.x + pos.pose.pose.position.y
def receiveRealPose(self, data):
try:
(trans, rot) = self.listener.lookupTransform('world', 'minefield', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
q = [rot[0], rot[1], rot[2], rot[3]]
roll, pitch, yaw = euler_from_quaternion(q)
#frame = PyKDL.Frame(PyKDL.Rotation.RPY(roll, pitch, yaw), PyKDL.Vector(trans[0],trans[1],trans[2]))
q = [data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w]
roll, pitch, robot_yaw = euler_from_quaternion(q)
#robot = PyKDL.Frame(PyKDL.Rotation.RPY(roll, pitch, robot_yaw), PyKDL.Vector(data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z))
#robot_transformed = frame * robot;
#[roll, pitch, yaw] = robot_transformed.M.GetRPY()
#print data.pose.pose.position.x - trans[0], data.pose.pose.position.y - trans[1], robot_yaw - yaw
pose = [data.pose.pose.position.x - trans[0], data.pose.pose.position.y - trans[1], robot_yaw - yaw]
self.updateRobotPose(pose)
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 updateImu(self, imu):
config = self.config
config.imu_last_update = imu.header.stamp #rospy.Time.now()
config.imu_init = True
if not config.imu_data :
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = config.imu_tf.M * imu_data
angle = imu_data.GetRPY()
config.last_imu_orientation = angle
config.last_imu_update = imu.header.stamp
# Initialize heading buffer in order to apply a savitzky_golay derivation
if len(config.heading_buffer) == 0:
config.heading_buffer.append(angle[2])
inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
config.heading_buffer.append(config.heading_buffer[-1] + inc)
if len(config.heading_buffer) == len(config.savitzky_golay_coeffs):
config.imu_data = True
else:
angle = euler_from_quaternion([imu.orientation.x, imu.orientation.y, imu.orientation.z, imu.orientation.w])
imu_data = PyKDL.Rotation.RPY(angle[0], angle[1], angle[2])
imu_data = config.imu_tf.M * imu_data
angle = imu_data.GetRPY()
angle_quaternion = tf.transformations.quaternion_from_euler(angle[0], angle[1], angle[2])
config.odom.pose.pose.orientation.x = angle_quaternion[0]
config.odom.pose.pose.orientation.y = angle_quaternion[1]
config.odom.pose.pose.orientation.z = angle_quaternion[2]
config.odom.pose.pose.orientation.w = angle_quaternion[3]
# Derive angular velocities from orientations #######################
period = (imu.header.stamp - config.last_imu_update).to_sec()
# For yaw rate we apply a savitzky_golay derivation
inc = cola2_lib.normalizeAngle(angle[2] - config.heading_buffer[-1])
config.heading_buffer.append(config.heading_buffer[-1] + inc)
config.heading_buffer.pop(0)
config.odom.twist.twist.angular.z = convolve(config.heading_buffer, config.savitzky_golay_coeffs, mode='valid') / period
# TODO: Roll rate and Pitch rate should be also savitzky_golay derivations
config.odom.twist.twist.angular.x = cola2_lib.normalizeAngle(angle[0] - config.last_imu_orientation[0]) / period
config.odom.twist.twist.angular.y = cola2_lib.normalizeAngle(angle[1] - config.last_imu_orientation[1]) / period
config.last_imu_orientation = angle
config.last_imu_update = imu.header.stamp
#####################################################################
try:
self.LOCK.acquire()
if self.makePrediction(imu.header.stamp):
self.filter.updatePrediction()
self.publishData()
finally:
self.LOCK.release()
def callback(human_data, robot_data):
# TODO: Calculate yaw
human_quaternion = [human_data.pose.pose.orientation.x, human_data.pose.pose.orientation.y, human_data.pose.pose.orientation.z, human_data.pose.pose.orientation.w]
robot_quaternion = [robot_data.pose.pose.orientation.x, robot_data.pose.pose.orientation.y, robot_data.pose.pose.orientation.z, robot_data.pose.pose.orientation.w]
human_yaw = euler_from_quaternion(human_quaternion)[2]
robot_yaw = euler_from_quaternion(robot_quaternion)[2]
print("%s,%s,%s,%s,%s,%s,%s" %(human_data.header.stamp, human_data.pose.pose.position.x,human_data.pose.pose.position.y, human_yaw,robot_data.pose.pose.position.x,robot_data.pose.pose.position.y, robot_yaw))
def receiveOdomEKF(self, odor):
xm ,ym, (pitchm, rollm, yawm) = 549848.663622 , 4448426.10338, euler_from_quaternion([0.,0.,0.999998918808,0.00147050426938])
q = [odor.pose.pose.orientation.x, odor.pose.pose.orientation.y, odor.pose.pose.orientation.z, odor.pose.pose.orientation.w]
roll, pitch, yaw = euler_from_quaternion(q)
x, y = odor.pose.pose.position.x, odor.pose.pose.position.y
x1 = (x-xm)*cos(yawm) + (y-ym)*sin(yawm)
y1 = -(x-xm)*sin(yawm) + (y-ym)*cos(yawm)
self.odometria = [x1, y1, yaw-yawm]
self.updateRobotPose(self.odometria)
def convert_push_trial_to_feat_vectors(trial):
Z = []
Y = []
for i in xrange(len(trial.trial_trajectory)-1):
cts_t0 = trial.trial_trajectory[i]
cts_t1 = trial.trial_trajectory[i+1]
[_, _, ee_phi_t0] = euler_from_quaternion(np.array([cts_t0.ee.orientation.x,
cts_t0.ee.orientation.y,
cts_t0.ee.orientation.z,
cts_t0.ee.orientation.w]))
[_, _, ee_phi_t1] = euler_from_quaternion(np.array([cts_t1.ee.orientation.x,
cts_t1.ee.orientation.y,
cts_t1.ee.orientation.z,
cts_t1.ee.orientation.w]))
# TODO: This will depend on the primitive used
u_phi_world = cts_t0.u.angular.x
z_t = [cts_t0.x.x,
cts_t0.x.y,
cts_t0.x.theta,
cts_t0.ee.position.x,
cts_t0.ee.position.y,
cts_t0.ee.position.z,
ee_phi_t0,
cts_t0.u.linear.x,
cts_t0.u.linear.y,
cts_t0.u.linear.z,
u_phi_world]
# Convert world frame feats into object frame
z_t_obj_frame = get_object_frame_features(cts_t0, ee_phi_t0)
# TODO: Convert shape into frame for each instance?
# TODO: Need to extract this first...
z_shape = trial.trial_start.shape_descriptor
z_t.extend(z_t_obj_frame)
z_t.extend(z_shape)
# World frame
y_t = [cts_t1.x.x - cts_t0.x.x,
cts_t1.x.y - cts_t0.x.y,
cts_t1.x.theta - cts_t0.x.theta,
cts_t1.ee.position.x - cts_t0.ee.position.x,
cts_t1.ee.position.y - cts_t0.ee.position.y,
cts_t1.ee.position.z - cts_t0.ee.position.z,
ee_phi_t1 - ee_phi_t0,
cts_t1.t - cts_t0.t]
# Transform targets into object frame here...
y_t_obj_frame = get_object_frame_targets(cts_t0, cts_t1)
y_t.extend(y_t_obj_frame)
Z.append(z_t)
Y.append(y_t)
return (Z, Y)
def get_rot_matrix(self, rev=False):
frm, to = 'ardrone/odom', 'ardrone/ardrone_base_bottomcam'
if rev:
frm, to = to, frm
self.tf.waitForTransform(frm, to, rospy.Time(0), rospy.Duration(3))
trans, rot = self.tf.lookupTransform(frm, to, rospy.Time(0))
x, y, z = euler_from_quaternion(rot)
yaw = euler_from_quaternion(rot, axes='szxy')[0]
return yaw, np.array(euler_matrix(-x, -y, z))
def increment_reference(self):
'''
Steps the model reference (trajectory to track) by one self.timestep.
'''
# Frame management quantities
R_ref = trns.quaternion_matrix(self.q_ref)[:3, :3]
y_ref = trns.euler_from_quaternion(self.q_ref)[2]
# Convert body PD gains to world frame
kp = R_ref.dot(self.kp_body).dot(R_ref.T)
kd = R_ref.dot(self.kd_body).dot(R_ref.T)
# Compute error components (desired - reference), using "smartyaw"
p_err = self.p_des - self.p_ref
v_err = -self.v_ref
w_err = -self.w_ref
if npl.norm(p_err) <= self.heading_threshold:
q_err = trns.quaternion_multiply(self.q_des, trns.quaternion_inverse(self.q_ref))
else:
q_direct = trns.quaternion_from_euler(0, 0, np.angle(p_err[0] + (1j * p_err[1])))
q_err = trns.quaternion_multiply(q_direct, trns.quaternion_inverse(self.q_ref))
y_err = trns.euler_from_quaternion(q_err)[2]
# Combine error components into error vectors
err = np.concatenate((p_err, [y_err]))
errdot = np.concatenate((v_err, [w_err]))
wrench = (kp.dot(err)) + (kd.dot(errdot))
# Compute world frame thruster matrix (B) from thruster geometry, and then map wrench to thrusts
B = np.concatenate((R_ref.dot(self.thruster_directions.T), R_ref.dot(self.lever_arms.T)))
B_3dof = np.concatenate((B[:2, :], [B[5, :]]))
command = self.thruster_mapper(wrench, B_3dof)
wrench_saturated = B.dot(command)
# Use model drag to find drag force on virtual boat
twist_body = R_ref.T.dot(np.concatenate((self.v_ref, [self.w_ref])))
D_body = np.zeros_like(twist_body)
for i, v in enumerate(twist_body):
if v >= 0:
D_body[i] = self.D_body_positive[i]
else:
D_body[i] = self.D_body_negative[i]
drag_ref = R_ref.dot(D_body * twist_body * abs(twist_body))
# Step forward the dynamics of the virtual boat
self.a_ref = (wrench_saturated[:2] - drag_ref[:2]) / self.mass_ref
self.aa_ref = (wrench_saturated[5] - drag_ref[2]) / self.inertia_ref
self.p_ref = self.p_ref + (self.v_ref * self.timestep)
self.q_ref = trns.quaternion_from_euler(0, 0, y_ref + (self.w_ref * self.timestep))
self.v_ref = self.v_ref + (self.a_ref * self.timestep)
self.w_ref = self.w_ref + (self.aa_ref * self.timestep)
def odomCallback(msg):
global firstTime
global targetPoint
global targetQuat
if not reverse:
if len(prevPoints) == 0:
prevPoints.append(msg.pose.pose.position)
prevQuats.append(msg.pose.pose.orientation)
point0 = prevPoints[-1]
point1 = msg.pose.pose.position
dist = math.sqrt(math.pow(point0.x - point1.x, 2) + math.pow(point0.y - point1.y, 2))
if(dist > threshDist):
prevPoints.append(point1)
prevQuats.append(msg.pose.pose.orientation)
else:
if firstTime:
targetPoint = prevPoints.pop()
targetQuat = prevQuats.pop()
firstTime = False
currPoint = msg.pose.pose.position
currQuat = msg.pose.pose.orientation
quaternion = (currQuat.x, currQuat.y, currQuat.z, currQuat.w)
euler = euler_from_quaternion(quaternion)
currYaw = euler[2]
quaternion = (targetQuat.x, targetQuat.y, targetQuat.z, targetQuat.w)
euler = euler_from_quaternion(quaternion)
targetYaw = euler[2]
tw = Twist()
#print str(abs(currYaw - targetYaw))
if abs(currYaw - targetYaw) < 0.1:
if abs(targetPoint.x - currPoint.x) < 0.1 and abs(targetPoint.y - currPoint.y) < 0.1:
if len(prevPoints) != 0:
targetPoint = prevPoints.pop()
targetQuat = prevQuats.pop()
tw.linear.x = 0
tw.angular.z = 0
else:
tw.linear.x = -0.2
tw.angular.z = 0
else:
print str(radDeg(currYaw)) + ", " + str(radDeg(targetYaw))
# TODO fix rotation direction to rotate optimally
if radDeg(targetYaw - currYaw) > 180.0:
tw.angular.z = 0.5
else:
tw.angular.z = -0.5
tw.linear.x = 0
pub.publish(tw)
def listen_imu_quaternion(self, imu):
"""Doesn't work b/c desired yaw is too far from possible yaw"""
current_orientation = np.array([imu.orientation.x,
imu.orientation.y,
imu.orientation.z,
imu.orientation.w])
# current_angular_speed = np.array([imu.angular_velocity.x,
# imu.angular_velocity.y,
# imu.angular_velocity.z])
diff_orientation = transformations.quaternion_multiply(
self.desired_orientation,
# Unit quaternion => inverse = conjugate / norm = congugate
transformations.quaternion_conjugate(current_orientation))
assert np.allclose(
transformations.quaternion_multiply(diff_orientation,
current_orientation),
self.desired_orientation)
# diff_r, diff_p, diff_y = transformations.euler_from_quaternion(
# diff_orientation)
# rospy.loginfo("Orientation error (quaternion): {}".format(diff_orientation))
# rospy.loginfo(
# "Orientation error (deg): {}".format(
# np.rad2deg([diff_r, diff_p, diff_y]))
# )
# out = self.pitch_controller(diff_p)
corrected_orientation = transformations.quaternion_multiply(
quaternion_power(diff_orientation, 1.5),
self.desired_orientation)
rospy.loginfo(
"Desired orientation (deg): {}".format(
np.rad2deg(transformations.euler_from_quaternion(
self.desired_orientation))))
rospy.loginfo(
"Corrected orientation (deg): {}".format(
np.rad2deg(transformations.euler_from_quaternion(
corrected_orientation))))
rospy.loginfo("Desired position: {}".format(
self.desired_position))
setpoint_pose = Pose(self.desired_position, corrected_orientation)
servo_angles = self.platform.ik(setpoint_pose)
rospy.logdebug(
"Servo angles (deg): {}".format(np.rad2deg(servo_angles)))
self.publish_servo_angles(servo_angles)
def process_relative_speed(self,new_segment, segment_input,last_segment):
pos = np.array([segment_input.pose.pose.position.x,segment_input.pose.pose.position.y,0,0])
last_pos = np.array([last_segment.pose.pose.position.x,last_segment.pose.pose.position.y,0,0])
angle = euler_from_quaternion([segment_input.pose.pose.orientation.x,
segment_input.pose.pose.orientation.y,
segment_input.pose.pose.orientation.z,
segment_input.pose.pose.orientation.w])[2]
last_angle = euler_from_quaternion([last_segment.pose.pose.orientation.x,
last_segment.pose.pose.orientation.y,
last_segment.pose.pose.orientation.z,
last_segment.pose.pose.orientation.w])[2]
[a,b] = frame_change(pos,last_pos,angle, last_angle, self.delta)
new_segment.twist.twist.linear.x = a[0] #linear speed in direction of the person
new_segment.twist.twist.linear.y = a[1] #lateral speed at perpendicular of direction of the person
new_segment.twist.twist.angular.y = b #angular yaw speed
def saveMapToFile(map, yaml_file, image_file, negate, free_thresh,
occupied_thresh):
map_info = {}
map_info['resolution'] = map.info.resolution
origin = [0.0, 0.0, 0.0]
origin[0] = map.info.origin.position.x
origin[1] = map.info.origin.position.y
a = euler_from_quaternion([
map.info.origin.orientation.x,
map.info.origin.orientation.y,
map.info.origin.orientation.z,
map.info.origin.orientation.w
])
origin[2] = a[2] #yaw
map_info['origin'] = origin
map_info['negate'] = 1 if negate else 0
map_info['occupied_thresh'] = occupied_thresh
map_info['free_thresh'] = free_thresh
map_info['image'] = image_file
if image_file[0] != '/':
yaml_file_dir = os.path.dirname(os.path.realpath(yaml_file))
image_file = yaml_file_dir + '/' + image_file
saveMapToImageFile(map, image_file, negate, free_thresh, occupied_thresh)
with open(yaml_file, "w") as outfile:
outfile.write(yaml.dump(map_info))
outfile.close()
def odometryCb(self,msg):
#print 'llega mensaje de odometria'
self.x=msg.pose.pose.position.x
self.y=msg.pose.pose.position.y
self.ang=euler_from_quaternion([msg.pose.pose.orientation.x,msg.pose.pose.orientation.y,msg.pose.pose.orientation.z,msg.pose.pose.orientation.w])[2]
self.realSpeed=msg.twist.twist.linear.x
self.realTurn=msg.twist.twist.angular.z
def quaternion_to_euler_yaw(self, orientation):
_, _, yaw = euler_from_quaternion(
(orientation.x, orientation.y, orientation.z, orientation.w))
return yaw
def get_euler(pose):
"""Returns the roll, pitch yaw angles from a Quaternion """
return transformations.euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
def rotation_manager(self, imu_data):
self.check_station_keep_timer()
if self.navigation_indicator == True and len(self.current_coordinates)==2:
global roll, pitch, yaw
q = imu_data.orientation
# print(q)
q_list = [q.x, q.y, q.z, q.w]
# To convert from quat to euler angles
(roll, pitch, yaw) = euler_from_quaternion (q_list)
# print("ROLL")
# print(roll)
# print("PITCH")
# print(pitch)
# print("YAW")
# print(yaw)
# To convert from euler to quat
# quat = quaternion_from_euler (roll, pitch, yaw)
# print quat
# if self.imu_orientation == "NED":
# yaw_rad = atan2(2.0 * (q.y*-q.x + q.w*q.z), q.w*q.w - q.z*q.z - q.y*q.y + q.x*q.x)
# else:
# yaw_rad = atan2(2.0 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z)
# yaw_rad = atan2(2.0 * (q.y*q.z + q.w*q.x), q.w*q.w - q.x*q.x - q.y*q.y + q.z*q.z)
yaw_rad = atan2(2.0 * (q.y*-q.x + q.w*q.z), q.w*q.w - q.z*q.z - q.y*q.y + q.x*q.x)
# print("yaw rad")
# print(yaw_rad)
# yaw_deg = (yaw_rad+3.14159/2)*-180/3.14159
yaw_deg = yaw_rad*(180/3.14159)
# yaw_deg -= imu_degree_offset_bias # YAW in terms of north, gets larger when going cw
# print("yaw_deg: " + str(yaw_deg))
# yaw_deg *= -1
# yaw_deg += 180
# print("yaw deg")
# print(yaw_deg)
# print("yaw_deg post_process")
theta_offset = self.caclulate_rotation(self.current_coordinates, self.desired_coordinates, yaw_deg)
theta_offset *= -1
theta_offset += self.imu_degree_offset_bias
# print("Theta Offset")
# print(theta_offset)
# theta_offset -= 180
# if theta_offset > 180:
# theta_offset -= 360
# elif theta_offset < -180:
# theta_offset += 360
# print(theta_offset)
if -5 < theta_offset < 5:
self.degree_offset = 0
# print("no degree offset")
else:
self.degree_offset = theta_offset
# print(self.degree_offset)
# print("Theta OFfset")
# print(theta_offset)
# print("Distance:")
# print(self.distance)
self.print_progress()
def getYaw(quat):
try:
return euler_from_quaternion([quat.x, quat.y, quat.z, quat.w])[2]
except AttributeError:
return euler_from_quaternion(quat)[2]
def position_callback(self, msg): quaternion = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w] self.yaw = euler_from_quaternion(quaternion)[2]
trans_front_side = tfBuffer.lookup_transform(
'camera', 'front_side', rospy.Time(), rospy.Duration(1))
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rate.sleep()
continue
timer = trans_front_side.header.stamp.secs
if (rospy.get_time() - timer) < 1.5:
bot_pos = geometry_msgs.msg.Vector3()
quat = [
trans_front_side.transform.rotation.x,
trans_front_side.transform.rotation.y,
trans_front_side.transform.rotation.z,
trans_front_side.transform.rotation.w
]
pitch, yaw, roll = euler_from_quaternion(quat)
bot_pos.x = trans_front_side.transform.translation.z
bot_pos.y = yaw #CCW angle off pos x from dump bucket corner, still needs stepper transform
bot_pos.z = trans_front_side.transform.translation.x - 0.25
if math.fabs(roll) > 2:
bot_pos.z = -bot_pos.z
bot_pos.y = -yaw
else:
bot_pos.y = -42
pospub.publish(bot_pos)
rate.sleep()
def get_rotation (self,orientation_q):
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
return yaw
def optimizeManeuver(self):
'''
Sets up the optimization problem then calculates the optimal maneuver
poses. Publishes the resulting path if the optimization is successful.
Args:
-----
msg: ROS Bool message
Returns:
--------
path: the optimal path as a ROS nav_msgs/Path message
'''
# Make sure a target pose exists
if (self.target_x is not None):
# Grab the current pose from the recent transform if there is no 'odom' topic being published to
if (self.current_pose is None):
# DEBUG:
print(
"No 'odom' message received. Waiting for transform from 'odom' to 'base_link'..."
)
listener = tf.TransformListener()
try:
listener.waitForTransform('/odom', '/base_link',
rospy.Time(0),
rospy.Duration(10))
(trans,
rot) = listener.lookupTransform('/odom', '/base_link',
rospy.Time(0))
self.current_pose = Pose()
self.current_pose.position.x = trans[0]
self.current_pose.position.y = trans[1]
self.current_pose.position.z = trans[2]
self.current_pose.orientation.x = rot[0]
self.current_pose.orientation.y = rot[1]
self.current_pose.orientation.z = rot[2]
self.current_pose.orientation.w = rot[3]
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
return False, "Error looking up transform from 'odom' to 'base_link'"
# DEBUG:
print("Running maneuver optimization...")
# Initial value for optimization
x0 = [self.start_x_s, self.start_y_s]
lower_bounds = [-2, -16]
upper_bounds = [20, 5]
# Set params
# TODO: add the forklifts current pose from "/odom"
current_pose2D = Pose2D()
current_pose2D.x = self.current_pose.position.x
current_pose2D.y = self.current_pose.position.y
euler_angles = euler_from_quaternion([
self.current_pose.orientation.x,
self.current_pose.orientation.y,
self.current_pose.orientation.z,
self.current_pose.orientation.w
])
current_pose2D.theta = euler_angles[2]
params = {
"current_pose":
[current_pose2D.x, current_pose2D.y, current_pose2D.theta],
"forklift_length": (self.base_to_back + self.base_to_clamp),
"weights": [10, 1, 0.1, 1],
"obstacles":
self.obstacles,
"min_radius":
self.min_radius
}
print("Using optimization method: %d" % self.optimization_method)
#==================================================================#
# vvv Add Autograd gradient functions here if you get to it vvv
#==================================================================#
# Generate Gradient Functions
self.grad_maneuverObjective = grad(
lambda x: self.maneuverObjective(x, params))
self.hessian_maneuverObjective = hessian(
lambda x: self.maneuverObjective(x, params))
self.jac_maneuverIneqConstraints = jacobian(
lambda x: self.maneuverIneqConstraints(x, params))
self.hessian_maneuverIneqConstraints = hessian(
lambda x: self.maneuverIneqConstraints(x, params))
# # Test Gradients against finite difference method
# delta = 0.0000001
# x = np.array([self.start_x_s, self.start_y_s], dtype=np.float)
# dx = deepcopy(x)
# dx[0] = x[0] + delta
# print("Objective: ")
# print(self.maneuverObjective(x, params))
# print(self.maneuverObjective(dx, params))
# print("Autograd:")
# print(self.grad_maneuverObjective(x))
# print("Finite Difference:")
# print((self.maneuverObjective(dx, params) - self.maneuverObjective(x, params))/delta)
# print("Hessian:")
# print(self.hessian_maneuverObjective(x))
# print("Autograd con:")
# print(self.jac_maneuverIneqConstraints(x))
# print("Constraint Jacobian:")
# print(self.gradManeuverIneqConstraints(x, params))
# print("Hessian con:")
# print(self.hessian_maneuverIneqConstraints(x))
#==================================================================#
# ^^^ Add Autograd gradient functions here if you get to it ^^^
#==================================================================#
#==================================================================#
# scipy.optimize.minimize optimizer
#==================================================================#
if (self.optimization_method == 1):
# Set up optimization problem
obj = lambda x: self.maneuverObjective(x, params)
obj_bfgs = lambda x: self.maneuverObjective(x, params)
ineq_con = {
'type': 'ineq',
'fun': lambda x: self.maneuverIneqConstraints(x, params),
'jac': None
}
bounds = [(lower_bounds[0], upper_bounds[0]),
(lower_bounds[1], upper_bounds[1])]
# Optimize
tic = time.time()
#res = minimize(obj, x0, jac=self.grad_maneuverObjective, method='SLSQP', bounds=bounds, constraints=ineq_con)
#res = minimize(obj, x0, method='SLSQP', bounds=bounds, constraints=ineq_con)
res = minimize(obj_bfgs,
x0,
method='COBYLA',
bounds=bounds,
constraints=ineq_con)
toc = time.time()
# DEBUG:
print("===== Optimization Results =====")
print("time: %f(sec)" % (toc - tic))
print("Success: %s" % res.success)
print("Message: %s" % res.message)
print("Results:\n x: %f, y: %f" % (res.x[0], res.x[1]))
# Store result
x_s = res.x[0]
y_s = res.x[1]
# Update starting point to be the current result
self.start_x_s = x_s
self.start_y_s = y_s
self.optimization_success = res.success
message = res.message
#==================================================================#
# scipy.optimize.minimize optimizer
#==================================================================#
#==================================================================#
# IPOPT Optimizer
#==================================================================#
if (self.optimization_method == 2):
# Initial value for optimization
x0_ip = np.array([x0[0], x0[1]])
nvar = 2
x_L = np.array(lower_bounds, dtype=np.float_)
x_U = np.array(upper_bounds, dtype=np.float_)
ncon = 1
g_L = np.array([0], dtype=np.float_)
g_U = np.array([0], dtype=np.float_)
nnzj = nvar * ncon
nnzh = nvar**2
def eval_f(x):
return self.maneuverObjective(x, params)
def eval_grad_f(x):
return self.grad_maneuverObjective(x)
def eval_g(x):
return self.maneuverIneqConstraints(x, params)
def eval_jac_g(x, flag):
if flag:
rows = np.concatenate(
(np.ones(nvar) * 0, np.ones(nvar) * 1))
cols = np.concatenate(
(np.linspace(0, nvar - 1, nvar),
np.linspace(nvar, 2 * nvar - 1, nvar)))
return (rows, cols)
else:
return self.jac_maneuverIneqConstraints(x)
def eval_h(x, lagrange, obj_factor, flag):
if flag:
rows = np.array([])
for i in range(nvar * ncon):
rows = np.concatenate((rows, np.ones(nvar) * i))
cols = np.array([])
for i in range(nvar * ncon):
cols = np.concatenate(
(cols, np.linspace(0, nvar - 1, nvar)))
return (rows, cols)
else:
constraint_hessian = self.hessian_maneuverIneqConstraints(
x)
constraint_sum = lagrange[0] * constraint_hessian[
0, :, :]
constraint_sum = constraint_sum + lagrange[
1] * constraint_hessian[1, :, :]
return obj_factor * self.hessian_maneuverObjective(
x) + constraint_sum
# Not using hessian, remove this line when using it
nnzh = 0
nlp = pyipopt.create(nvar, x_L, x_U, ncon, g_L, g_U, nnzj,
nnzh, eval_f, eval_grad_f, eval_g,
eval_jac_g)
pyipopt.set_loglevel(0)
tic = time.time()
x, zl, zu, constraint_multipliers, obj, status = nlp.solve(
x0_ip)
nlp.close()
toc = time.time()
def print_variable(variable_name, value):
for i in range(len(value)):
print("{} {}".format(
variable_name + "[" + str(i) + "] =", value[i]))
print("Solution of the primal variables, x")
print_variable("x", x)
#
# print("Solution of the bound multipliers, z_L and z_U")
# print_variable("z_L", zl)
# print_variable("z_U", zu)
#
# print("Solution of the constraint multipliers, lambda")
# print_variable("lambda", constraint_multipliers)
#
# print("Objective value")
# print("f(x*) = {}".format(obj))
# DEBUG:
print("===== Optimization Results (IPOPT) =====")
print("time: %f" % (toc - tic))
print("Success: %s" % status)
print("Message: %s" % "")
print("Results:\n x: %f, y: %f" % (x[0], x[1]))
# Store result
x_s = x[0]
y_s = x[1]
self.optimization_success = (status > 0)
message = "ipopt optimization finished with status: {0:d}".format(
status)
#==================================================================#
# IPOPT Optimizer
#==================================================================#
#=================================================================#
# Use hardcoded value
#=================================================================#
if (self.optimization_method == 0):
x_s = x0[0]
y_s = x0[1]
self.optimization_success = 1
message = "used hardcoded starting value"
#=================================================================#
# Use hardcoded value
#=================================================================#
# Print optimized point
print("Approach starting point: (%0.4f, %0.4f)" % (x_s, y_s))
# Set initial pose angle for the forklift to be the direction facing the roll
theta_s = np.arctan2(self.target_y - y_s, self.target_x - x_s)
# Initialize path messages
current_time = rospy.Time.now()
path1_msg = Path()
path2_msg = Path()
path1_gear_msg = PathWithGear()
path2_gear_msg = PathWithGear()
path1_msg.header.stamp = current_time
path1_msg.header.frame_id = "odom"
path2_msg.header.stamp = current_time
path2_msg.header.frame_id = "odom"
path1_gear_msg.path.header.stamp = current_time
path1_gear_msg.path.header.frame_id = "odom"
path2_gear_msg.path.header.stamp = current_time
path2_gear_msg.path.header.frame_id = "odom"
# Publish first segment of maneuver
# NOTE: Just set the path to be a single point at the current position. This will make the master controller work the same and quickly move through the two maneuver paths
point = PoseStamped()
point.header.frame_id = "odom"
point.pose.position.x = self.current_pose.position.x
point.pose.position.y = self.current_pose.position.y
path1_msg.poses.append(point)
path1_gear_msg.path.poses.append(point)
# Set gear, positive alpha = forward gear
self.path1_pub.publish(path1_msg)
path1_gear_msg.gear = 1
self.path1_gear_pub.publish(path1_gear_msg)
# Publish second segment of maneuver
point = PoseStamped()
point.header.frame_id = "odom"
point.pose.position.x = self.current_pose.position.x
point.pose.position.y = self.current_pose.position.y
path2_msg.poses.append(point)
path2_gear_msg.path.poses.append(point)
# Set gear, positive alpha = forward gear
self.path2_pub.publish(path2_msg)
path2_gear_msg.gear = 1
self.path2_gear_pub.publish(path2_gear_msg)
if (self.optimization_success):
# If optimization was successful, publish the new target
# position for the A* algorithm (you will want to make this a
# separate "goal" value distinct from the roll target position)
rospy.set_param("/control_panel_node/goal_x", float(x_s))
rospy.set_param("/control_panel_node/goal_y", float(y_s))
self.update_obstacle_end_pose = False
# Publish the starting pose for the approach b-spline path
approach_start_pose = PoseStamped()
approach_start_pose.header.frame_id = "/odom"
approach_start_pose.pose.position.x = x_s
approach_start_pose.pose.position.y = y_s
quat_forklift = quaternion_from_euler(0, 0, wrapToPi(theta_s))
approach_start_pose.pose.orientation.x = quat_forklift[0]
approach_start_pose.pose.orientation.y = quat_forklift[1]
approach_start_pose.pose.orientation.z = quat_forklift[2]
approach_start_pose.pose.orientation.w = quat_forklift[3]
self.approach_pose_pub.publish(approach_start_pose)
return self.optimization_success, message
else:
return False, "No target pose exists"
def updateOdom(odom):
global robotLocation
global currentHeading
robotLocation = np.array([odom.pose.pose.position.x, odom.pose.pose.position.y])
orientation = euler_from_quaternion((odom.pose.pose.orientation.x, odom.pose.pose.orientation.y, odom.pose.pose.orientation.z, odom.pose.pose.orientation.w))
currentHeading = orientation[2]
initialise_pose_position() #should only be used to initialised
initialise_pose_orientation() #should only be used to initialised
#obs_move = [4,4,4,3,3,3,0,0,0,0,0,0,0,7,2,2,7,4,7,4,7,4,7,4,0,0,7,4,7,4,7,4,7]
obs_move = [
3, 7, 2, 10, 11, 3, 0, 5, 2, 7, 7, 9, 3, 9, 7, 8, 10, 6, 8, 9, 1, 4, 2, 0,
11, 2, 10, 7, 2, 5
]
for i, index in enumerate(obs_move):
print(index)
moveSawyer(index)
moveTo(final_pose)
test.unregister()
'''
if not moveSawyer(index):
print("Invalid Path")
break
'''
print("Endpoint cartesian")
cart = [pose.position.x, pose.position.y, pose.position.z]
print(cart)
print("Endpoint orientation")
orient = [
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]
print(orient)
print("Endpoint orientation in Euler")
euler = list(euler_from_quaternion(orient))
print(euler)
def get_yaw_from_msg(msg):
q = [msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w]
euler = transformations.euler_from_quaternion(q)
return euler[2]
def callback(data):
global NEIGHBORHOOD_LENGTH
global NUM_BOXES
global STEP_SIZE
global PUB
global K
global BUFFER
startTime = datetime.now()
boxes_to_buffer = int(math.ceil(BUFFER / NEIGHBORHOOD_LENGTH * NUM_BOXES))
#print("LOCAL RRT IS CALLING BACK REGARDING OCCUPANCY")
#print(np.asarray(data.grid_path).shape)
grid_map = np.asarray(data.grid_path).reshape(NUM_BOXES, NUM_BOXES)
grid_map = np.flipud(grid_map)
buffered_map = bufferizeManhattan(grid_map, boxes_to_buffer)
x = data.current_odometry.pose.pose.position.x
y = data.current_odometry.pose.pose.position.y
euler = euler_from_quaternion(
(data.current_odometry.pose.pose.orientation.x,
data.current_odometry.pose.pose.orientation.y,
data.current_odometry.pose.pose.orientation.z,
data.current_odometry.pose.pose.orientation.w))
yaw = euler[2]
#print("let's make the local rrt")
localRRT = LocalRRT(buffered_map.astype(int), K, [x, y], yaw,
[data.next_point.x, data.next_point.y])
localRRT_no_buffer = LocalRRT(grid_map.astype(int), K, [x, y], yaw,
[data.next_point.x, data.next_point.y])
#print("do we need to run local rrt?")
if not (localRRT_no_buffer.local_map.isValidEdge(
[x, y], [data.next_point.x, data.next_point.y],
print_statements=True)):
print("let's run the local rrt")
local_path = localRRT.runRRT(STEP_SIZE, 100).reshape((-1, 2))
while (len(local_path) < 2):
print("THIS PATH IS TOO SHORT")
break
boxes_to_buffer = boxes_to_buffer - 1
if (boxes_to_buffer < 0):
print("COULD NOT FIND A PATH")
break
buffered_map = bufferizeManhattan(grid_map, boxes_to_buffer)
localRRT = LocalRRT(buffered_map.astype(int), K, [x, y], yaw,
[data.next_point.x, data.next_point.y])
local_path = localRRT.runRRT(STEP_SIZE, 100).reshape((-1, 2))
# Send this update
result_msg = local_rrt_result()
result_msg.follow_local_path = True
local_path = np.flipud(local_path)
result_msg.global_path_x = local_path[:, 0].tolist()
result_msg.global_path_y = local_path[:, 1].tolist()
result_msg.next_point = data.next_point
#print("NEW LOCAL PATH")
#print(local_path)
PUB.publish(result_msg)
return
else:
result_msg = local_rrt_result()
result_msg.follow_local_path = False
PUB.publish(result_msg)
print("No need to update")
def moveSawyer(index):
if index == 0:
pose.position.x += 0.05
elif index == 1:
pose.position.x -= 0.05
elif index == 2:
pose.position.y += 0.05
elif index == 3:
pose.position.y -= 0.05
elif index == 4:
pose.position.z += 0.05
elif index == 5:
pose.position.z -= 0.05
#can't seem to make it move properly.
elif index == 6:
#atm it euler rotates at 10 degrees
temp_euler = list(
euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
temp_euler[0] += 0.2618 #5 degrees
temp_ori = quaternion_from_euler(temp_euler[0], temp_euler[1],
temp_euler[2])
pose.orientation.x = temp_ori[0]
pose.orientation.y = temp_ori[1]
pose.orientation.z = temp_ori[2]
pose.orientation.w = temp_ori[3]
elif index == 7:
temp_euler = list(
euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
temp_euler[0] -= 0.2618
temp_ori = quaternion_from_euler(temp_euler[0], temp_euler[1],
temp_euler[2])
pose.orientation.x = temp_ori[0]
pose.orientation.y = temp_ori[1]
pose.orientation.z = temp_ori[2]
pose.orientation.w = temp_ori[3]
elif index == 8:
temp_euler = list(
euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
temp_euler[1] += 0.2618 #0.0873 5 degrees
temp_ori = quaternion_from_euler(temp_euler[0], temp_euler[1],
temp_euler[2])
pose.orientation.x = temp_ori[0]
pose.orientation.y = temp_ori[1]
pose.orientation.z = temp_ori[2]
pose.orientation.w = temp_ori[3]
elif index == 9:
temp_euler = list(
euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
temp_euler[1] -= 0.2618 #0.0873
temp_ori = quaternion_from_euler(temp_euler[0], temp_euler[1],
temp_euler[2])
pose.orientation.x = temp_ori[0]
pose.orientation.y = temp_ori[1]
pose.orientation.z = temp_ori[2]
pose.orientation.w = temp_ori[3]
elif index == 10:
temp_euler = list(
euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
temp_euler[2] += 0.2618
temp_ori = quaternion_from_euler(temp_euler[0], temp_euler[1],
temp_euler[2])
pose.orientation.x = temp_ori[0]
pose.orientation.y = temp_ori[1]
pose.orientation.z = temp_ori[2]
pose.orientation.w = temp_ori[3]
elif index == 11:
temp_euler = list(
euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
]))
temp_euler[2] -= 0.2618
temp_ori = quaternion_from_euler(temp_euler[0], temp_euler[1],
temp_euler[2])
pose.orientation.x = temp_ori[0]
pose.orientation.y = temp_ori[1]
pose.orientation.z = temp_ori[2]
pose.orientation.w = temp_ori[3]
if moveTo(pose):
return True
else:
return False
def get_rotation (msg): global roll, pitch, yaw orientation_q = msg.pose.pose.orientation orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w] (roll, pitch, yaw) = euler_from_quaternion (orientation_list)
def transform_to_list(tf):
a = euler_from_quaternion(msg_to_quaternion(tf.rotation))
t = tf.translation
return [t.x, t.y, t.z, a[0], a[1], a[2]]
def __init__(self):
# Read parameters to configure the node
tf_publish_rate = read_parameter('~tf_publish_rate', 50.)
tf_period = 1. / tf_publish_rate if tf_publish_rate > 0 else float(
'inf')
parent_frame = read_parameter('~parent_frame', 'world')
optitrack_frame = read_parameter('~optitrack_frame', 'optitrack')
rigid_bodies = read_parameter('~rigid_bodies', dict())
names = []
ids = []
for name, value in rigid_bodies.items():
names.append(name)
ids.append(value)
# Setup Publishers
pose_pub = rospy.Publisher('/optitrack/rigid_bodies',
RigidBodyArray,
queue_size=3)
# Setup TF listener and broadcaster
tf_listener = tf.TransformListener()
tf_broadcaster = tf.TransformBroadcaster()
# Connect to the optitrack system
iface = read_parameter('~iface', 'eth1')
version = (2, 7, 0, 0) # the latest SDK version
#IPython.embed()
#optitrack = rx.mkdatasock(ip_address=get_ip_address(iface))
#Modified by Nikhil
# optitrack = rx.mkdatasock(ip_address=iface)
optitrack = rx.mkcmdsock(ip_address=iface)
msg = struct.pack("I", rx.NAT_PING)
server_address = '192.168.1.205'
result = optitrack.sendto(msg, (server_address, rx.PORT_COMMAND))
rospy.loginfo('Successfully connected to optitrack')
# Get transformation from the world to optitrack frame
(parent, child) = (parent_frame, optitrack_frame)
try:
now = rospy.Time.now() + rospy.Duration(1.0)
tf_listener.waitForTransform(parent, child, now,
rospy.Duration(5.0))
(pos, rot) = tf_listener.lookupTransform(parent, child, now)
wTo = PoseConv.to_homo_mat(pos, rot) # return 4x4 numpy mat
except (tf.Exception, tf.LookupException, tf.ConnectivityException):
rospy.logwarn('Failed to get transformation from %r to %r frame' %
(parent, child))
parent_frame = optitrack_frame
wTo = np.eye(4)
# Track up to 24 rigid bodies
prevtime = np.ones(24) * rospy.get_time()
# IPython.embed()
while not rospy.is_shutdown():
try:
# data = optitrack.recv(rx.MAX_PACKETSIZE)
data, address = optitrack.recvfrom(rx.MAX_PACKETSIZE + 4)
except socket.error:
if rospy.is_shutdown(): # exit gracefully
return
else:
rospy.logwarn('Failed to receive packet from optitrack')
msgtype, packet = rx.unpack(data, version=version)
if type(packet) is rx.SenderData:
version = packet.natnet_version
rospy.loginfo('NatNet version received: ' + str(version))
if type(packet) in [rx.SenderData, rx.ModelDefs, rx.FrameOfData]:
# Optitrack gives the position of the centroid.
array_msg = RigidBodyArray()
# IPython.embed()
if msgtype == rx.NAT_FRAMEOFDATA:
# IPython.embed()
for i, rigid_body in enumerate(packet.rigid_bodies):
body_id = rigid_body.id
pos_opt = np.array(rigid_body.position)
rot_opt = np.array(rigid_body.orientation)
# IPython.embed()
oTr = PoseConv.to_homo_mat(pos_opt, rot_opt)
# Transformation between world frame and the rigid body
WTr = np.dot(wTo, oTr)
wTW = np.array([[0, -1, 0, 0], [1, 0, 0, 0],
[0, 0, 1, 0], [0, 0, 0, 1]])
wTr = np.dot(wTW, WTr)
## New Change ##
# Toffset = np.array( [[0, 1, 0, 0],[0, 0, 1, 0],[1, 0, 0, 0],[0, 0, 0, 1]] )
# tf_wTr = np.dot(oTr,Toffset)
# tf_pose = Pose()
# tf_pose.position = Point(*tf_wTr[:3,3])
# tf_pose.orientation = Quaternion(*tr.quaternion_from_matrix(tf_wTr))
# IPython.embed()
array_msg.header.stamp = rospy.Time.now()
array_msg.header.frame_id = parent_frame
body_msg = RigidBody()
pose = Pose()
pose.position = Point(*wTr[:3, 3])
# OTr = np.dot(oTr,Toffset)
# pose.orientation = Quaternion(*tr.quaternion_from_matrix(oTr))
# change 26 Feb. 2017
# get the euler angle we want then compute the new quaternion
euler = tr.euler_from_quaternion(rot_opt)
quaternion = tr.quaternion_from_euler(
euler[2], euler[0], euler[1])
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
body_msg.id = body_id
body_msg.tracking_valid = rigid_body.tracking_valid
body_msg.mrk_mean_error = rigid_body.mrk_mean_error
body_msg.pose = pose
for marker in rigid_body.markers:
# TODO: Should transform the markers as well
body_msg.markers.append(Point(*marker))
array_msg.bodies.append(body_msg)
# Control the publish rate for the TF broadcaster
if rigid_body.tracking_valid and (
rospy.get_time() - prevtime[body_id] >=
tf_period):
body_name = 'rigid_body_%d' % (body_id)
if body_id in ids:
idx = ids.index(body_id)
body_name = names[idx]
## no change ##
# tf_broadcaster.sendTransform(pos_opt, rot_opt, rospy.Time.now(), body_name, optitrack_frame)
# change 1 ##
# pos2 = np.array([tf_pose.position.x, tf_pose.position.y, tf_pose.position.z])
# rot2 = np.array([tf_pose.orientation.x, tf_pose.orientation.y, tf_pose.orientation.z, tf_pose.orientation.w])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 2 ## <fail>
# pos2 = np.array([-pose.position.y, pose.position.x, pose.position.z])
# rot2 = np.array([-pose.orientation.y, pose.orientation.x, pose.orientation.z, pose.orientation.w])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 3 ## <fail>
# pos2 = np.array([-pos_opt[1],pos_opt[0],pos_opt[2]])
# rot2 = np.array([-rot_opt[1],rot_opt[0],rot_opt[2],rot_opt[3]])
# tf_broadcaster.sendTransform(pos2, rot2, rospy.Time.now(), body_name, optitrack_frame)
## change 4 ## <>
pos2 = np.array([
pose.position.x, pose.position.y,
pose.position.z
])
rot2 = np.array([
pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w
])
tf_broadcaster.sendTransform(
pos2, rot2, rospy.Time.now(), body_name,
parent_frame)
prevtime[body_id] = rospy.get_time()
pose_pub.publish(array_msg)
self.orientation_queue = []
rospy.set_param('docking', True)
print("Service request received.")
return "Service requested."
if __name__ == "__main__":
my_filter = Pose_filter()
while (not rospy.is_shutdown()):
# if STATION_NR is provided
if (my_filter.marker_pose_calibrated.pose.position.x
and rospy.get_param('docking')):
# implementing a temporal sliding window filter here
[position_vec, orient_vec
] = my_filter.vec_from_pose(my_filter.marker_pose_calibrated.pose)
euler_vec = euler_from_quaternion(orient_vec)
my_filter.position_queue.append(position_vec)
my_filter.orientation_queue.append(orient_vec)
filtered_position_vec = my_filter.avr(my_filter.position_queue)
filtered_orient_vec = my_filter.avr(my_filter.orientation_queue)
filtered_pose = my_filter.pose_from_vec(filtered_position_vec,
filtered_orient_vec)
my_filter.filtered_pose_pub.publish(filtered_pose)
#my_filter.filtered_pose_pub.publish(my_filter.marker_pose_calibrated)
# discard the oldest element in queue
if (len(my_filter.position_queue) == 15):
my_filter.position_queue.pop(0)
my_filter.orientation_queue.pop(0)
# provide an output to remind user of starting docking at correct position(rosservice call)
# wouldn't be necessary if choose to change docking into autonomous process
elif (my_filter.marker_list
gps_n = xyz()
dist = []
myrosbag = rosbags[inp]
print("Working on: %s" % (myrosbag))
bag = rosbag.Bag(myrosbag, "r")
count = 0
for topic, msg, t in bag.read_messages(topics=[
'/current_pose', '/current_pose_alt', '/gps/duro/current_pose',
'/gps/nova/current_pose'
]):
if topic == '/current_pose':
gps_p.x.append(msg.pose.position.x)
gps_p.y.append(msg.pose.position.y)
gps_p.t.append(t.to_sec())
_, _, yaw = euler_from_quaternion([
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
])
gps_p.ori.append(yaw)
elif topic == '/current_pose_alt':
gps_a.x.append(msg.pose.position.x)
gps_a.y.append(msg.pose.position.y)
gps_a.t.append(t.to_sec())
_, _, yaw = euler_from_quaternion([
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
])
gps_a.ori.append(yaw)
if len(gps_p.x): # if not empty
## 20 Hz = 0.05 second
time_margin = 0.018
if abs(gps_p.t[-1] - t.to_sec()) < time_margin:
def stageCB(self, data):
#if len(data.laser.ranges) < 1 or self.readyForNewData == False:
#print "limitted by GPU !!!!"
# return
# pose stuff
self.robotPose = data.position.pose
quatOri = (self.robotPose.orientation.x, self.robotPose.orientation.y,
self.robotPose.orientation.z, self.robotPose.orientation.w)
(_, _, yaw) = transformations.euler_from_quaternion(quatOri)
robotOri = 180 * yaw / np.pi
angleBetween = 180 * np.arctan2(
self.goalPose.position.y - self.robotPose.position.y,
self.goalPose.position.x - self.robotPose.position.x) / np.pi
dirToTarget = self.constrainAngle(robotOri - angleBetween)
self.screen[:] = 128
x = self.observation_dims[0] / 2
y = self.observation_dims[1] / 2
rad = int(data.laser.range_max * 10)
# Occupancy grid:
for i in range(0, 360):
for j in range(0, rad):
if data.laser.ranges[i] * 10 >= j:
self.setPixel(x + int(j * np.cos(np.pi * i / 180.0)),
y + int(j * np.sin(np.pi * i / 180.0)), 192)
if data.laser.ranges[i] < data.laser.range_max:
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)),
y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)),
0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)),
y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)) -
1, 0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)),
y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)) -
2, 0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)) -
1, y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)),
0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)) -
1, y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)) -
1, 0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)) -
1, y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)) -
2, 0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)) -
2, y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)),
0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)) -
2, y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)) -
1, 0)
self.setPixel(
x +
int(10 * data.laser.ranges[i] * np.cos(np.pi * i / 180)) -
2, y +
int(10 * data.laser.ranges[i] * np.sin(np.pi * i / 180)) -
2, 0)
# Gradient:
# R = np.matrix('{} {}; {} {}'.format(np.cos( np.radians( -dirToTarget)), -np.sin( np.radians( -dirToTarget)), np.sin( np.radians( -dirToTarget)), np.cos( np.radians( -dirToTarget))))
tempScr = np.zeros(
(self.observation_dims[0], self.observation_dims[1]))
for i in range(0, self.observation_dims[0]):
for j in range(0, self.observation_dims[1]):
x = int(
(i - self.observation_dims[0] / 2) * np.cos(np.pi + yaw) +
(j - self.observation_dims[1] / 2) * -np.sin(np.pi + yaw))
y = int(
(i - self.observation_dims[0] / 2) * np.sin(np.pi + yaw) +
(j - self.observation_dims[1] / 2) * np.cos(np.pi + yaw))
tempScr[i][j] = np.sqrt((self.robotPose.position.x + x / 10 -
self.goalPose.position.x)**2 +
(self.robotPose.position.y + y / 10 -
self.goalPose.position.y)**2)
tempScr -= np.min(tempScr)
tempScr /= np.max(tempScr)
for i in range(0, self.observation_dims[0]):
for j in range(0, self.observation_dims[1]):
self.addPixel(i, j, 63.0 * (1 - tempScr[i][j]))
if data.collision == True:
self.terminal = 1
self.minFrontDist = data.minFrontDist
self.readyForNewData = False
def odom_callback(self, msg):
self.x0 = msg.pose.pose.position.x
self.y0 = msg.pose.pose.position.y
_, _, self.yaw0 = euler_from_quaternion((msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w))
self.odom_received = True
def get_yaw(msg): #radians
global roll, pitch, yaw
orientation_q = msg.orientation
orientation_list = [orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion (orientation_list)
return yaw
def getRotation(odomMsg):
orientation_q = odomMsg.pose.pose.orientation
orientation_list = [ orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
return yaw
def localizeCallback(self, data):
mapToLaser = self.tfListener.lookupTransform('/map', '/laser',
rospy.Time(0))
toLaserTransMatrix = np.asarray(mapToLaser[0][:2])
carOrientQTMap = (data.pose.orientation.x, data.pose.orientation.y,
data.pose.orientation.z, data.pose.orientation.w)
carYawMap = euler_from_quaternion(carOrientQTMap)[2] #yaw
toLaserRotMatrix = np.array([[np.cos(carYawMap), -np.sin(carYawMap)],
[np.sin(carYawMap),
np.cos(carYawMap)]])
toLaser = lambda coords: (coords - toLaserTransMatrix).dot(
toLaserRotMatrix)
#finding goal waypoint and other metadata
carPositionMap = np.array([data.pose.position.x, data.pose.position.y])
carPositionLaser = toLaser(
carPositionMap) #this should be the zero vector
#apply positional extrapolation based on localization delay
extrap = self.extrapolatePosition()
self.LOOKAHEAD_DISTANCE = self.decideLookahead()
print(self.lookSpeedMultiplier())
print(self.lookAngleMultiplier())
pathPointsLaser = toLaser(np.asarray(self.path_points))
distances = np.linalg.norm(pathPointsLaser - carPositionLaser, axis=1)
mask = np.ones(len(distances), dtype=int)
viable = np.where(
np.logical_and((distances >= self.LOOKAHEAD_DISTANCE),
(pathPointsLaser[:, 0] >
(self.FOV_MULT * pathPointsLaser[:, 1]))))
mask[viable] = 0
viableMask = ma.masked_array(distances, mask=mask)
waypointIndex = viableMask.argmin()
waypoint = pathPointsLaser[waypointIndex]
goalX = waypoint[0]
goalY = waypoint[1]
#calculate goal angle and velocity
self.findCurveRadius = lambda distance, offset: distance**2 / (
2 * np.abs(offset))
self.turnRadius = self.findCurveRadius(distances[waypointIndex], goalY)
self.curvature = 1 / self.turnRadius
self.steeringAngle = np.arcsin(
self.WHEELBASE / self.turnRadius) * np.sign(goalY)
self.steeringAngle = np.clip(self.steeringAngle, -self.MAX_TURN_ANGLE,
self.MAX_TURN_ANGLE)
print("===========================")
print("X: " + str(goalX))
print("Y: " + str(goalY))
print("R: " + str(self.turnRadius))
print("angle: " + str(self.steeringAngle))
print("ex: " + str(extrap[0]))
print("ey: " + str(extrap[1]))
print("LOOK: " + str(self.decideLookahead()))
print("SPD: " + str(self.decideVelocity()))
print("SPD-EST: " + str(self.currentSpeed))
print("===========================")
msg = drive_param()
msg.velocity = self.decideVelocity()
msg.angle = self.steeringAngle
self.drivePub.publish(msg)
def _get_imu(self, msg): #listen to /atlas/imu/pose/pose/orientation
roll, pitch, self._yaw = euler_from_quaternion([
msg.orientation.x, msg.orientation.y, msg.orientation.z,
msg.orientation.w
])
self._CD_StateMachine.UpdateOdometryYaw(self._yaw)
def process(self):
global FIND_FACE
if self.robot_pose is None:
return
print("Now State: ", self.state)
msg_pose = PoseStamped()
# find human face
if self.state == 0:
if self.face_pose == None:
return
msg_pose.pose = self.face_pose
if -0.23 < self.face_pose.position.x < 0.23 and -0.2 < self.face_pose.position.y < 0.2:
self.state = 1
self.fg.position.x = self.robot_pose.position.x
self.fg.position.y = self.robot_pose.position.y
FIND_FACE = False
elif self.state == 1:
msg_pose.pose.position.x = 0
self.pubtarget.publish(msg_pose)
print("state 1 sleep")
time.sleep(7)
self.state = 2
# find cream
elif self.state == 2:
if self.cream_pose == None:
return
msg_pose.pose = self.cream_pose
print("cx cy", self.cream_pose.position.x, self.cream_pose.position.y)
if -0.23 < self.cream_pose.position.x < 0.23 and -0.05 < self.cream_pose.position.y < 0.05:
self.state = 3
elif self.state == 3:
print("State 3")
msg_pose.pose.position.x = 0.1
msg_pose.pose.position.y = 0
self.pubtarget.publish(msg_pose)
time.sleep(3)
msg_pose.pose.position.x = 0
self.pubtarget.publish(msg_pose)
print("state 3 sleep")
time.sleep(7)
self.state = 4
elif self.state == 4:
if self.face_pose == None:
return
if FIND_FACE == True:
msg_pose.pose = self.face_pose
else:
trans = [self.robot_pose.position.x,self.robot_pose.position.y,self.robot_pose.position.z]
rot = [self.robot_pose.orientation.x,self.robot_pose.orientation.y,self.robot_pose.orientation.z,self.robot_pose.orientation.w]
transformation_matrix = tr.compose_matrix(angles = tr.euler_from_quaternion(rot), translate = trans)
trans_c = [self.fg.position.x,self.fg.position.y,0.08]
rot_c = [0, 0, 0, 1]
mat_c = tr.compose_matrix(angles = tr.euler_from_quaternion(rot_c), translate = trans_c)
inv_mat = tr.inverse_matrix(transformation_matrix)
new_mat = np.dot(inv_mat, mat_c)
trans_new = tf.transformations.translation_from_matrix(new_mat)
msg_pose.pose.position.x = trans_new[0]
msg_pose.pose.position.y = trans_new[1]
print("face x: ", trans_new[0])
print("face y: ", trans_new[1])
if -0.23 < msg_pose.pose.position.x < 0.23 and -0.23 < msg_pose.pose.position.y < 0.2:
self.state = 5
elif self.state == 5:
msg_pose.pose.position.x = 0
self.pubtarget.publish(msg_pose)
def modify_pose_rotation(pose,
offset=0.0,
reference_axis='z',
rotation_range=None):
"""
Modifies the orientation of a pose, for a single rotation axis (reference_axis),
by adding an offset and limiting the rotation to be within certain range. If no
rotation_range is specified, the pose is not modified (except for the specified
offset).
:param pose: The pose to be modified.
:type pose: geometry_msgs.msg.PoseStamped
:param offset: Rotation offset to add to the reference_axis of the pose (in degrees).
:type offset: float
:param reference_axis: The rotation axis of the pose to be modified (e.g. x, y, z).
:type reference_axis: str
:param rotation_range: If specified, the rotation will be limited until the
rotation_range value (in degrees). For instance, if rotation_range is set
to [0, 180], where 0 is the minimum and 180 degrees is the maximum, the
rotation will remain in half circle (e.g. this parameter can be helpful
for symmetric objects, since there a 180 degree rotation makes no difference).
:type rotation_range: list
:return: The modified pose.
:rtype: geometry_msgs.msg.PoseStamped
"""
pose_out = copy.deepcopy(pose)
orientation_in = (pose_out.pose.orientation.x, pose_out.pose.orientation.y,
pose_out.pose.orientation.z, pose_out.pose.orientation.w)
angles_in = transformations.euler_from_quaternion(orientation_in)
# Add 360 degrees to the negative angles, since the range of
# euler_from_quaternion is -PI to PI (-180 to 180 degrees)
angles_in = [(math.pi * 2) + angle if angle < 0.0 else angle
for angle in angles_in]
euler_angles = {
'x': angles_in[0],
'y': angles_in[1],
'z': angles_in[2],
}
# Ensure the offset is not more than 360 degrees
offset %= 360
target_angle = euler_angles[reference_axis] + math.radians(offset)
if rotation_range is not None:
target_angle = restrict_angle_to_range(
target_angle, math.radians(offset),
list(numpy.radians(rotation_range)))
euler_angles[reference_axis] = target_angle
angles_out = [euler_angles['x'], euler_angles['y'], euler_angles['z']]
orientation_out = transformations.quaternion_from_euler(
angles_out[0], angles_out[1], angles_out[2])
#pose_out.pose.orientation.x = orientation_out[0]
#pose_out.pose.orientation.y = orientation_out[1]
#pose_out.pose.orientation.z = orientation_out[2]
#pose_out.pose.orientation.w = orientation_out[3]
#hardcoded way - always same position while lying down
print 'HORIZONTAL OBJECT'
# pose_out.pose.orientation.x = -0.71
# pose_out.pose.orientation.y = -0.7
# pose_out.pose.orientation.z = 0.0
# pose_out.pose.orientation.w = 0.0
pose_out.pose.orientation.x = -0.67559
pose_out.pose.orientation.y = -0.7372
pose_out.pose.orientation.z = 0.0
pose_out.pose.orientation.w = 0.0
return pose_out
def quatToAng3D(self,quat):
euler = euler_from_quaternion((quat.x,quat.y,quat.z,quat.w))
return euler
def convert_pose_to_xy_and_theta(self, pose):
""" Convert pose (geometry_msgs.Pose) to a (x,y,yaw) tuple """
orientation_tuple = (pose.orientation.x, pose.orientation.y,
pose.orientation.z, pose.orientation.w)
angles = t.euler_from_quaternion(orientation_tuple)
return (pose.position.x, pose.position.y, angles[2])
def from_urdf(cls,
urdf_string,
transmission_suffix='_thruster_transmission'):
'''
Load from an URDF string. Expects each thruster to be connected a transmission ending in the specified suffix.
A transform between the propeller joint and base_link must be available
'''
urdf = URDF.from_xml_string(urdf_string)
buff = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(buff) # noqa
names = []
joints = []
positions = []
angles = []
limit = -1
ratio = -1
for transmission in urdf.transmissions:
find = transmission.name.find(transmission_suffix)
if find != -1 and find + len(transmission_suffix) == len(
transmission.name):
if len(transmission.joints) != 1:
raise Exception(
'Transmission {} does not have 1 joint'.format(
transmission.name))
if len(transmission.actuators) != 1:
raise Exception(
'Transmission {} does not have 1 actuator'.format(
transmission.name))
t_ratio = transmission.actuators[0].mechanicalReduction
if ratio != -1 and ratio != t_ratio:
raise Exception(
'Transmission {} has a different reduction ratio (not supported)'
.format(t_ratio))
ratio = t_ratio
joint = None
for t_joint in urdf.joints:
if t_joint.name == transmission.joints[0].name:
joint = t_joint
if joint is None:
rospy.logerr('Transmission joint {} not found'.format(
transmission.joints[0].name))
try:
trans = buff.lookup_transform('wamv/base_link',
joint.child, rospy.Time(),
rospy.Duration(10))
except tf2_ros.TransformException as e:
raise Exception(e)
translation = rosmsg_to_numpy(trans.transform.translation)
rot = rosmsg_to_numpy(trans.transform.rotation)
yaw = euler_from_quaternion(rot)[2]
names.append(transmission.name[0:find])
positions.append(translation[0:2])
angles.append(yaw)
joints.append(joint.name)
if limit != -1 and joint.limit.effort != limit:
raise Exception(
'Thruster {} had a different limit, cannot proceed'.
format(joint.name))
limit = joint.limit.effort
limit_tuple = (limit, -limit)
return cls(names,
positions,
angles,
generate_linear_force_to_command(ratio),
limit_tuple,
joints=joints)
def quaternion_to_rpy(quat):
quatarray = quaternion_to_array(quat)
rpyarray = transformations.euler_from_quaternion(quatarray)
return (rpyarray[0], rpyarray[1], rpyarray[2])
