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()