def generate_pose(self, angle):
# Generate a pose, all values but the yaw will be 0.
q = transformations.quaternion_from_euler(0, 0, angle)
header = Header(
stamp=rospy.Time.now(),
frame_id="map"
)
pose = Pose(
position=Point(x=0, y=0, z=0),
orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
)
# Publish pose stamped - just for displaying in rviz
self.pose_est_pub.publish(
PoseStamped(
header=header,
pose=pose
)
)
# Publish pose with covariance stamped.
p_c_s = PoseWithCovarianceStamped()
p_c = PoseWithCovariance()
covariance = np.array([1, 0, 0, 0, 0, 0,
0, 1, 0, 0, 0, 0,
0, 0, 1, 0, 0, 0,
0, 0, 0, 1, 0, 0,
0, 0, 0, 0, 1, 0,
0, 0, 0, 0, 0, .7])**2
p_c.pose = pose
p_c.covariance = covariance
p_c_s.header = header
p_c_s.pose = p_c
# Publish pose estimation
self.p_c_s_est_pub.publish(p_c_s)
def publish_pose(self,pose,time):
#Generate pose
q = tf.transformations.quaternion_from_euler(0, 0, pose[2])
header = Header(
stamp=rospy.Time.now(),
frame_id="map"
)
pose = Pose(
position=Point(
x=pose[0],
y=pose[1],
z=0
),
orientation=Quaternion(
x=q[0],
y=q[1],
z=q[2],
w=q[3],
)
)
# Publish pose stamped
self.pose_est_pub.publish(
PoseStamped(
header=header,
pose=pose
)
)
# Publish pose with covariance stamped.
p_c_s = PoseWithCovarianceStamped()
p_c = PoseWithCovariance()
# These don't matter
covariance = np.array([ 1, 0, 0, 0, 0, 0,
0,.2*self.std_err, 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, self.std_err])**2
p_c.pose = pose
p_c.covariance = covariance
p_c_s.header = header
p_c_s.header.frame_id = "map"
p_c_s.pose = p_c
# if time.time() - self.start_time < 5:
# self.p_c_s_init_pub.publish(p_c_s)
# else:
self.p_c_s_est_pub.publish(p_c_s)
def _push_mocap_pose(self,data):
if self.tf.frameExists(self.frame) and self.tf.frameExists("/world"):
t = self.tf.getLatestCommonTime(self.frame, "/world")
position, quaternion = self.tf.lookupTransform("/world", self.frame, t)
msg = PoseWithCovarianceStamped()
pose = PoseWithCovariance()
pose.pose.position.x = position[0]
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
pose.pose.orientation.x = quaternion[0]
pose.pose.orientation.y = quaternion[1]
pose.pose.orientation.z = quaternion[2]
pose.pose.orientation.w = quaternion[3]
pose.covariance = [0] * 36 #could tune the covariance matrices?
pose.covariance[0] = -1
msg.pose = pose
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "/bug/base_link"
self.pose_converted.publish(msg)
def setestimate():
pub = rospy.Publisher('initialpose',
PoseWithCovarianceStamped,
queue_size=1)
p = PoseWithCovarianceStamped()
msg = PoseWithCovariance()
msg.pose = Pose(Point(0, 0, 0.190), Quaternion(0.000, 0.000, 0.223, 0.975))
msg.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.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
]
p.pose = msg
rate = rospy.Rate(10)
index = 0
while index < 10: # This is hacky hacky Rik 24-11-2015
index += 1
pub.publish(p)
rate.sleep()
def publish_pose(self,pose,stamp):
#Generate pose
q = tf.transformations.quaternion_from_euler(0, 0, pose[2])
header = Header(
stamp=stamp,
frame_id="map"
)
pose = Pose(
position=Point(x=pose[0], y=pose[1], z=0),
orientation=Quaternion( x=q[0], y=q[1], z=q[2], w=q[3])
)
# Publish pose stamped
self.pose_est_pub.publish(
PoseStamped(
header=header,
pose=pose
)
)
self.br.sendTransform((self.pose_est[0], self.pose_est[1], .125), q,
rospy.Time.now(),
"base_link",
"map")
# Publish pose with covariance stamped.
p_c_s = PoseWithCovarianceStamped()
p_c = PoseWithCovariance()
covariance = np.array([.05, 0, 0, 0, 0, 0,
0, .05, 0, 0, 0, 0,
0, 0, .05, 0, 0, 0,
0, 0, 0, .05, 0, 0,
0, 0, 0, 0, .05, 0,
0, 0, 0, 0, 0, .05])**2
p_c.pose = pose
p_c.covariance = covariance
p_c_s.header = header
p_c_s.pose = p_c
self.p_c_s_est_pub.publish(p_c_s)
def call_services():
time_now = rospy.Time.now()
poseWCS = PoseWithCovarianceStamped()
poseWCS.header.stamp = time_now
poseWCS.header.frame_id = '/map'
poseWCS.pose.pose = Pose(Point(-1, 4, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
poseWCS.pose.covariance = [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 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, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
]
odom = Odometry()
odom.header.stamp = time_now
odom.header.frame_id = '/map'
poseWC = PoseWithCovariance()
poseWC.pose = Pose(Point(-1, 4, 0.0), Quaternion(0.0, 0.0, 0.0, 1.0))
poseWC.covariance = [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 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, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
]
odom.pose = poseWC
twistWC = TwistWithCovariance()
twistWC.twist = Twist(Vector3(0.0, 0.0, 0.0), Vector3(0.0, 0.0, 0.0))
twistWC.covariance = [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 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, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942
]
odom.twist = twistWC
rospy.wait_for_service('/Pioneer3_vc_initialpose')
try:
initialpose_proxy = rospy.ServiceProxy('/Pioneer3_vc_initialpose',
InitialPoseToRobot)
resp = initialpose_proxy(poseWCS)
except rospy.ServiceException, e:
print '/Pioneer3_vc_initialpose service call failed: %s' % e
def callback(data):
global gohome, home, done_init
if home is None:
home = data.pose.pose
if not gohome:
pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped,
queue_size=1)
#rospy.loginfo("Setting Pose")
p = PoseWithCovarianceStamped()
msg = PoseWithCovariance()
#print data.pose.pose
msg.pose = data.pose.pose #Pose(Point(2.64534568787, 4.98263931274, 0.000), Quaternion(0.000, 0.000, -0.990151822567, 0.139997750521))
msg.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, 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
]
p.pose = msg
pub.publish(p)
def init_pose(self):
# Set up pose msg and send to /initialpose topic
init_pose = PoseWithCovariance()
init_pose.pose.position.x = GRID_LOCATIONS["red"][0]
init_pose.pose.position.y = GRID_LOCATIONS["red"][1]
init_pose.pose.position.z = 0.0
init_pose.pose.orientation.x = 0.0
init_pose.pose.orientation.y = 0.0
init_pose.pose.orientation.z = 0
init_pose.pose.orientation.w = 1.0
# covariance of particle cloud
covariance = [
0.16575166048810708, 0.005812119956448508, 0.0, 0.0, 0.0, 0.0,
0.005812119956448534, 0.163246490374612, 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,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05704654800158645
]
init_pose.covariance = covariance
init_pose_stamped = PoseWithCovarianceStamped()
init_pose_stamped.pose = init_pose
init_pose_stamped.header.frame_id = "map"
self.init_pose_pub.publish(init_pose_stamped)
def init_pose(self):
init_pose = PoseWithCovariance()
init_pose.pose.position.x = GRID_LOCATIONS[self.color][0]
init_pose.pose.position.y = GRID_LOCATIONS[self.color][1]
init_pose.pose.position.z = 0.0
q = quaternion_from_euler(0, 0, 0)
init_pose.pose.orientation.x = q[0]
init_pose.pose.orientation.y = q[1]
init_pose.pose.orientation.z = q[2]
init_pose.pose.orientation.w = q[3]
# particle cloud covariance
covariance = [
0.16575166048810708, 0.005812119956448508, 0.0, 0.0, 0.0, 0.0,
0.005812119956448534, 0.163246490374612, 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,
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05704654800158645
]
init_pose.covariance = covariance
init_pose_stamped = PoseWithCovarianceStamped()
init_pose_stamped.pose = init_pose
init_pose_stamped.header.frame_id = "map"
self.init_pose_pub.publish(init_pose_stamped)
rospy.sleep(2)
def publish_pose(self, particle_avg):
# Update pose and TF
self.pose_est = np.array(particle_avg)
#Generate pose
q = tf.transformations.quaternion_from_euler(0, 0, self.pose_est[2])
header = Header(stamp=rospy.Time.now(), frame_id="map")
pose = Pose(position=Point(x=self.pose_est[0],
y=self.pose_est[1],
z=.127),
orientation=Quaternion(
x=q[0],
y=q[1],
z=q[2],
w=q[3],
))
# Publish pose stamped
self.pose_est_pub.publish(PoseStamped(header=header, pose=pose))
# Publish pose with covariance stamped.
p_c_s = PoseWithCovarianceStamped()
p_c = PoseWithCovariance()
# These don't matter
covariance = np.array([
.01, 0, 0, 0, 0, 0, 0, .01, 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, .075
])**2
p_c.pose = pose
p_c.covariance = covariance
p_c_s.header = header
p_c_s.header.frame_id = "map"
p_c_s.pose = p_c
# if time.time() - self.start_time < 5:
# self.p_c_s_init_pub.publish(p_c_s)
# else:
self.p_c_s_est_pub.publish(p_c_s)
def start_ekf(self, position):
q = tf.transformations.quaternion_from_euler(0, 0, position[2])
header = Header(
stamp = rospy.Time.now(),
frame_id = "map"
)
pose = Pose(
position = Point(
x = position[0],
y = position[1],
z = 0
),
orientation = Quaternion(
x = q[0],
y = q[1],
z = q[2],
w = q[3],
)
)
# Publish pose with covariance stamped.
p_c_s = PoseWithCovarianceStamped()
p_c = PoseWithCovariance()
# These don't matter
covariance = np.array([.01, 0, 0, 0, 0, 0,
0, .01, 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, .0001]) ** 2
p_c.pose = pose
p_c.covariance = covariance
p_c_s.header = header
p_c_s.header.frame_id = "map"
p_c_s.pose = p_c
self.set_init_pose(p_c_s)
def packagePose(self, data):
out = PoseWithCovariance()
out.pose.position = listVector3Conversion(self.position)
out.pose.orientation = data.orientation
out.covariance = OdometryFaker.covariance
return out
def state_to_pose_with_cov(state_vec, cov):
out = PoseWithCovariance()
out.pose = state_2_pose(state_vec)
out.covariance = fill_pose_covariance(cov)
return out
def get_data():
global seq, abs_x, abs_y, abs_x_m, abs_y_m, last_time
seq += 1
m = sensor_read_motion()
if m.motion & 0x10:
print "Overflow"
qualityMsg.data = m.squal
qualityPub.publish(qualityMsg)
# if m.squal < 90:
# led_lighting.on()
# if m.squal > 90:
# led_lighting.off()
abs_x += m.dx
abs_y += m.dy
time_diff = rospy.get_time() - last_time
last_time = rospy.get_time()
# Convert the counts reading into metres
abs_x_m = (float(abs_x) / ADNS3080_COUNTS_PER_METER)
abs_y_m = (float(abs_y) / ADNS3080_COUNTS_PER_METER)
speed_x_m = (float(m.dx) / ADNS3080_COUNTS_PER_METER) / time_diff
speed_y_m = (float(m.dy) / ADNS3080_COUNTS_PER_METER) / time_diff
# print str(speed_x_m) + ", " + str(speed_y_m)
# print m.squal
# Y forward and back
msg.header.seq = seq
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "odom"
msg.child_frame_id = "base_link"
pose = Pose()
pose.position.x = abs_y_m
pose.position.y = abs_x_m
pose.position.z = 0
poseC = PoseWithCovariance()
poseC.pose = pose
poseC.covariance = [-1] * 36
msg.pose = poseC
# If we are pivoting the sensor doesnt work properly so don't output anything
if pivoting:
speed_y_m = 0
twist = Twist()
twist.linear.x = speed_y_m
twist.linear.y = 0
msg.twist.twist = twist
msg.twist.covariance = [0.01] * 36
odomPub.publish(msg)
rate.sleep()
def MeanCovToPoseWithCovariance(mean, cov):
pc = PoseWithCovariance()
pc.pose = TQToPose(mean[:3], RadianToQuat(mean[3:]))
pc.covariance = cov.ravel().tolist()
return pc
def spin(self):
dvl_data, dvl_data_valid = self.get_dvl_measurements()
if dvl_data_valid:
# Pressure measurements
measured_pressure = dvl_data["frames"][4]["inputs"][0]["lines"][0]["data"][0]
# Beam velocities and FOM (Figure Of Merit)
measured_velocity_beam1 = dvl_data["frames"][5]["inputs"][0]["lines"][0]["data"][0]
measured_velocity_beam2 = dvl_data["frames"][5]["inputs"][0]["lines"][1]["data"][0]
measured_velocity_beam3 = dvl_data["frames"][5]["inputs"][0]["lines"][2]["data"][0]
measured_velocity_beam4 = dvl_data["frames"][5]["inputs"][0]["lines"][3]["data"][0]
fom_beam1 = dvl_data["frames"][5]["inputs"][1]["lines"][0]["data"][0]
fom_beam2 = dvl_data["frames"][5]["inputs"][1]["lines"][1]["data"][0]
fom_beam3 = dvl_data["frames"][5]["inputs"][1]["lines"][2]["data"][0]
fom_beam4 = dvl_data["frames"][5]["inputs"][1]["lines"][3]["data"][0]
# Beam altitude measurements and FOM (Figure Of Merit)
measured_altitude_beam1 = dvl_data["frames"][5]["inputs"][2]["lines"][0]["data"][0]
measured_altitude_beam2 = dvl_data["frames"][5]["inputs"][2]["lines"][1]["data"][0]
measured_altitude_beam3 = dvl_data["frames"][5]["inputs"][2]["lines"][2]["data"][0]
measured_altitude_beam4 = dvl_data["frames"][5]["inputs"][2]["lines"][3]["data"][0]
# Velocity measurements and FOM in XYZ. Note the two measurements in Z
measured_velocity_x = dvl_data["frames"][6]["inputs"][0]["lines"][0]["data"][0]
measured_velocity_y = dvl_data["frames"][6]["inputs"][0]["lines"][1]["data"][0]
measured_velocity_z_1 = dvl_data["frames"][6]["inputs"][0]["lines"][2]["data"][0]
measured_velocity_z_2 = dvl_data["frames"][6]["inputs"][0]["lines"][3]["data"][0]
fom_velocity_x = dvl_data["frames"][6]["inputs"][1]["lines"][0]["data"][0]
fom_velocity_y = dvl_data["frames"][6]["inputs"][1]["lines"][1]["data"][0]
fom_velocity_z_1 = dvl_data["frames"][6]["inputs"][1]["lines"][2]["data"][0]
fom_velocity_z_2 = dvl_data["frames"][6]["inputs"][1]["lines"][3]["data"][0]
# Since we have two measurements for velocity in z, choose the best one
fom_velocity_z_best = 0
measured_velocity_z_best = 0
if fom_velocity_z_1 > fom_velocity_z_2:
fom_velocity_z_best = fom_velocity_z_2
measured_velocity_z_best = measured_velocity_z_2
else:
fom_velocity_z_best = fom_velocity_z_1
measured_velocity_z_best = measured_velocity_z_1
# Create twist part of odometry message
twist_msg = TwistWithCovariance()
twist_msg.twist.linear.x = measured_velocity_x
twist_msg.twist.linear.y = measured_velocity_y
twist_msg.twist.linear.z = measured_velocity_z_best
# Assume zero covariance, i.e. all non-diagonal elements are 0
# Squaring the FOM is used here as an estimate for variance
var_x = fom_velocity_x * fom_velocity_x
var_y = fom_velocity_y * fom_velocity_y
var_z = fom_velocity_z_best * fom_velocity_z_best
twist_msgcovariance = [var_x, 0, 0, 0, 0, 0,
0, var_y, 0, 0, 0, 0,
0, 0, var_z, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0]
# Create pose part of odometry message
# We only get z from the pressure measurement for the pose
# The pressure covariance is currently undetermined, but is
# set as exact for now. If the variance is found, it will be
# element the third diagonal element of the covariance matrix
pose_msg = PoseWithCovariance()
pose_msg.pose.position.z = self.pressure_to_depth(measured_pressure)
pose_msg.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]
odometry_msg = Odometry()
odometry_msg.header.seq = self.seq
odometry_msg.header.stamp = rospy.Time.now()
odometry_msg.header.frame_id = "dvl_link"
odometry_msg.pose = pose_msg
odometry_msg.twist = twist_msg
# Only publish odometry if valid velocity data
if (measured_velocity_x < self.invalid_velocity or
measured_velocity_y < self.invalid_velocity or
measured_velocity_z < self.invalid_velocity
):
rospy.logwarn("DVL velocity measurement invalid! Skipping.")
else:
self.odom_pub.publish(odometry_msg)
self.seq += 1
self.rate.sleep()
def on_message(client, userdata, msg):
#print(msg.topic+" "+str(msg.payload))
#print("topic: "+msg.topic)
#print(str(msg.payload))
y = json.loads(str(msg.payload))
_type = y["TYPE"]
if _type == "Req":
_msg = y["MSG"]
print(_msg)
if _msg == "GetPosition":
detail = {
"pos_x": robot_node.pose_x,
"pos_y": robot_node.pose_y,
"pos_z": robot_node.pose_z,
"ori_x": robot_node.roll,
"ori_y": robot_node.pitch,
"ori_z": robot_node.yaw
}
detailJson = json.dumps(detail)
x = {"TYPE": "Res", "MSG": "GetPosition", "DETAIL": detailJson}
strJson = json.dumps(x)
mqttc.publish("$aws/things/curobot/mobile", strJson, qos=1)
if _msg == "PutTargetPosition":
global isPutTargetPositiontarget
isPutTargetPosition = True
#print("PutTarget")
_detail = y["DETAIL"]
#print("PutTarget")
jsonDetail = json.loads(_detail)
#print(_detail)
nav_goal = PoseStamped()
nav_goal.header.frame_id = "map"
nav_goal.header.stamp = rospy.get_rostime()
nav_goal.pose.position.x = jsonDetail["pos_x"]
nav_goal.pose.position.y = jsonDetail["pos_y"]
nav_goal.pose.position.z = jsonDetail["pos_z"]
#print()
[q1, q2, q3, q4] = quaternion_from_euler(jsonDetail["ori_x"],
jsonDetail["ori_y"],
jsonDetail["ori_z"])
nav_goal.pose.orientation.x = q1
nav_goal.pose.orientation.y = q2
nav_goal.pose.orientation.z = q3
nav_goal.pose.orientation.w = q4
#print(q1)
pubGoal.publish(nav_goal)
#print("Pub Goal")
if _msg == "PutInitialPosition":
_detail = y["DETAIL"]
print("PutInitialPosition stage 1")
jsonDetail = json.loads(_detail)
print("PutInitialPosition stage 2")
print(_detail)
nav_initial_pose = PoseWithCovarianceStamped()
print("PutInitialPosition stage 3")
nav_initial_pose.header.frame_id = "map"
print("PutInitialPosition stage 4")
nav_initial_pose.header.stamp = rospy.get_rostime()
print("PutInitialPosition stage 5")
print(jsonDetail["pos_x"])
[q1, q2, q3, q4] = quaternion_from_euler(int(jsonDetail["ori_x"]),
int(jsonDetail["ori_y"]),
int(jsonDetail["ori_z"]))
p = PoseWithCovarianceStamped()
print("pub initial pose 1")
msg = PoseWithCovariance()
#print("pub initial pose 2")
msg.pose = Pose(
Point(float(jsonDetail["pos_x"]), float(jsonDetail["pos_y"]),
float(jsonDetail["pos_z"])), Quaternion(q1, q2, q3, q4))
print("pub initial pose 3")
msg.covariance = [
0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 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, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
0.06853892326654787
]
print("pub initial pose 4")
p.pose = msg
print("pub initial pose")
#pwc = PoseWithCovariance()
#pwc.pose = Pose(Point(float(jsonDetail["pos_x"]),float(jsonDetail["pos_y"]),float(jsonDetail["pos_z"])), Quaternionquaternion_from_euler(q1,q2,q3,q4))
#nav_initial_pose.pose.position.x = 12#float(jsonDetail["pos_x"])
#print("PutInitialPosition stage 6")
#nav_initial_pose.pose.position.y = int(jsonDetail["pos_y"])
#nav_initial_pose.pose.position.z = int(jsonDetail["pos_z"])
#print()
#print("PutInitialPosition stage 2")
#nav_initial_pose.pose.orientation.x = q1
#nav_initial_pose.pose.orientation.y = q2
#nav_initial_pose.pose.orientation.z = q3
#nav_initial_pose.pose.orientation.w = q4
#print("PutInitialPosition stage 3")
pubInitialPose.publish(p)
def publish(self, dt):
if self.robot.active:
msg = TeamData()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.robot_id = self.robot.robot_id
msg.robot_position.pose = self.robot.pose
msg.robot_position.covariance = [self.robot.covariance, 0, 0, 0, 0, 0,
0, self.robot.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, self.robot.covariance]
if self.robot.ball.active:
try:
# beware, we use transforms3d here which has quaternion in different format than ros
robot_xyzw = self.robot.pose.orientation
robot_mat_in_world = quat2mat((robot_xyzw.w, robot_xyzw.x, robot_xyzw.y, robot_xyzw.z))
robot_trans_in_world = compose(
(self.robot.pose.position.x, self.robot.pose.position.y, self.robot.pose.position.z),
robot_mat_in_world, [1, 1, 1])
world_trans_in_robot = np.linalg.inv(robot_trans_in_world)
ball_xyzw = self.robot.ball.pose.orientation
mat_in_world = quat2mat((ball_xyzw.w, ball_xyzw.x, ball_xyzw.y, ball_xyzw.z))
trans_in_world = compose((self.robot.ball.pose.position.x, self.robot.ball.pose.position.y,
self.robot.ball.pose.position.z),
mat_in_world, [1, 1, 1])
trans_in_robot = np.matmul(world_trans_in_robot, trans_in_world)
pos_in_robot, mat_in_robot, _, _ = decompose(trans_in_robot)
ball_relative = PoseWithCovariance()
ball_relative.pose.position = Point(*pos_in_robot)
ball_absolute = PoseWithCovariance()
ball_absolute.pose.position = self.robot.ball.pose.position
ball_absolute.pose.orientation = Quaternion(0, 0, 0, 1)
ball_absolute.covariance = [self.robot.ball.covariance, 0, 0, 0, 0, 0,
0, self.robot.ball.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, self.robot.ball.covariance]
msg.ball_absolute = ball_absolute
cartesian_distance = math.sqrt(
ball_relative.pose.position.x ** 2 + ball_relative.pose.position.y ** 2)
msg.time_to_position_at_ball = cartesian_distance / ROBOT_SPEED
except tf2_ros.LookupException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
except tf2_ros.ExtrapolationException as ex:
rospy.logwarn_throttle(10.0, rospy.get_name() + ": " + str(ex))
return None
self.pub.publish(msg)
else:
# ball not seen
msg.ball_relative.covariance = 100.0
msg.time_to_position_at_ball = 0.0
self.pub.publish(msg)
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)
# rospy.loginfo(checkFlag)
# rospy.loginfo(velocity)
pub01.publish(getOdometry)
def run(self):
"""Run the Distance Tracker with the input from the camera"""
bridge = CvBridge()
rate = rospy.Rate(24)
#for heading averaging
current_sum = 0.0
count = 0.0
current_slope = 0.0
last_point = 0.0
current_point = 0.0
average = 0.0
while not rospy.is_shutdown():
target_found, target_centroid, dist, offset = self.find_target()
#print("EST DISTANCE: " + str(dist) + ' inches')
#print("EST OFFSET: " + str(offset) + ' inches')
#print("--------------")
if target_found:
print("Found.")
#heading averaging
last_point = current_point
current_point = offset[0]
new_slope = (current_point - last_point)
if new_slope != 0:
new_slope = new_slope / abs(new_slope)
if current_slope == 1:
if new_slope < 0:
average = current_sum / count
current_sum = 0.0
count = 0.0
#print("slope is 1")
current_sum += current_point
count += 1
#slope can never be 0 (kind of hacky solution to the situation where
# 'current_slope' is 0 and 'new_slope' is negative, which would cause
# the program to not take the average)
if new_slope != 0:
current_slope = new_slope
#TODO pitch averaging
#TODO dist averaging
#IMU + Camera data fusion
#NOTE: All sensor data should in "sofi_cam" frame for now
#IMU
#1) Convert euler rates to linear velocities & publish separately
#2) Publish original Imu message
if self.imu_msg is not None:
self.twist_msg = TwistWithCovarianceStamped()
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = "base_link" #TODO convert to base_link frame
self.twist_msg.header = h
#Compute cross product of IMU angular velocities and distance to object to estimate object's tangential velocity
omega = [
-self.imu_msg.angular_velocity.x,
-self.imu_msg.angular_velocity.y,
-self.imu_msg.angular_velocity.z
]
r = [dist, offset[0], offset[1]]
v_tan = np.cross(omega, r)
#Publish TwistWithCovarianceStamped message
self.twist_msg.twist.twist.linear.x = v_tan[0]
self.twist_msg.twist.twist.linear.y = v_tan[1]
self.twist_msg.twist.twist.linear.z = v_tan[2]
self.twist_msg.twist.twist.angular.x = 0
self.twist_msg.twist.twist.angular.y = 0
self.twist_msg.twist.twist.angular.z = 0
self.twist_msg.twist.covariance = [
0.003, 0, 0, 0, 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0,
0.003, 0, 0, 0, 0, 0, 0, 0.1, 0, 0, 0, 0, 0, 0, 0.1, 0,
0, 0, 0, 0, 0, 0.1
] #TODO determine covariance matrix
self.twist_pub.publish(self.twist_msg)
#Publish Imu message (after flipping acceleration sign)
self.imu_msg.linear_acceleration.x = -self.imu_msg.linear_acceleration.x
self.imu_msg.linear_acceleration.y = -self.imu_msg.linear_acceleration.y
self.imu_msg.linear_acceleration.z = -self.imu_msg.linear_acceleration.z
self.imu_pub.publish(self.imu_msg)
#Publish PoseWithCovarianceStamped from raspicam
self.pose.header.seq = 1
self.pose.header.stamp = rospy.Time.now()
self.pose.header.frame_id = "odom"
p = PoseWithCovariance()
p.pose.position.x = dist
p.pose.position.y = offset[0]
p.pose.position.z = offset[1]
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
p.covariance = [
0.003, 0, 0, 0, 0, 0, 0, 0.003, 0, 0, 0, 0, 0, 0, 0.003, 0,
0, 0, 0, 0, 0, 0.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0.0
] #TODO determine covariance matrix
self.pose.pose = p
self.pose_pub.publish(self.pose)
self.found_pub.publish(True)
self.average_heading_pub.publish(average)
self.average_pitch_pub.publish(offset[1])
self.average_dist_pub.publish(dist)
else:
self.found_pub.publish(False)
#TODO IMU only (?)
rate.sleep()
