def write_bag(bag_origin, bag_write: rosbag.Bag, t_start): count = 0 for topic, msg, t in bag_origin.read_messages("/tf", t_start): time_stamp = rospy.Time(0) + (t - t_start) msg.transforms[0].header.stamp = time_stamp bag_write.write(topic, msg, time_stamp) count += 1 bag_write.close()
def process_bag(bag_in_fn, bag_out_fn, conf_file_fn): bag_in = Bag(bag_in_fn) bag_out = Bag(bag_out_fn, 'w') include_rules, exclude_rules, time_rules = read_rules(conf_file_fn) topic_rules = include_rules + exclude_rules # Set the time UNIX time at the start of the bag file for r in topic_rules: r.set_begin_time(bag_in.get_start_time()) for r in time_rules: r.set_begin_time(bag_in.get_start_time()) # Find start and end times that are actually required t_start, t_end = get_begin_end(time_rules, bag_in) # Find topics that are actually required bag_topics = bag_in.get_type_and_topic_info().topics.keys() topics = get_topics(topic_rules, bag_topics) messages = bag_in.read_messages(topics=topics, start_time=t_start, end_time=t_end, return_connection_header=True) for topic, msg, t, conn_header in messages: # Check default enforcement for this message if FilterRule.is_tf_topic(topic): default = FilterRule.DEFAULT_ENFORCEMENT_TF else: default = FilterRule.DEFAULT_ENFORCEMENT_TOPIC # When default is to include, only check whether the exclusion # rules are satisfied, and if all of them are ok, write it out if default == FilterRule.INCLUDE: # Check exclusions ok = True for r in exclude_rules: if r.msg_match(topic, msg, t): ok = False # When default is to exclude, check if the message matches any # of the inclusion rules and write it out if it does else: # Check inclusions ok = False for r in include_rules: if r.msg_match(topic, msg, t): ok = True # Time rules time_ok = True for r in time_rules: if not r.is_ok_with(topic, msg, t): time_ok = False # Write to file if ok and time_ok: bag_out.write(topic, msg, t, connection_header=conn_header) bag_out.close()
def ipfs_upload(messages): rospy.wait_for_service('/liability/infochan/ipfs/add_file') upload = rospy.ServiceProxy('/liability/infochan/ipfs/add_file', IpfsUploadFile) with NamedTemporaryFile(delete=False) as tmpfile: recorder = Bag(tmpfile.name, 'w') for topic in messages: recorder.write('/{}'.format(topic), messages[topic]) recorder.close() res = upload(Filepath(tmpfile.name)) if not res.success: raise Exception(res.error_msg) return res.ipfs_address
def create_objective(period: str, stream_id: str, email: str) -> str: ipfs = connect() tempdir = gettempdir() os.chdir(tempdir) with NamedTemporaryFile(delete=False) as f: bag = Bag(f.name, 'w') bag.write('/period', String(period)) bag.write('/stream_id', String(stream_id)) bag.write('/email', String(email)) bag.close() res = ipfs.add(f.name) rospy.loginfo("Hash of the objective is {}".format(res['Hash'])) return res['Hash']
def process_bag(bag_in_fn, bag_out_fn, conf_file_fn): bag_in = Bag(bag_in_fn) bag_out = Bag(bag_out_fn, 'w') topic_rules, tf_rules = RenameRule.parse(conf_file_fn) messages = bag_in.read_messages(return_connection_header=True) for topic, msg, t, conn_header in messages: if topic == '/tf' or topic == '/tf_static': new_topic = topic new_msg = modify_tf(msg, tf_rules) else: new_topic = modify_topic(topic, topic_rules) # Modify the frame_id in header if header exists new_msg = modify_msg(msg, tf_rules) bag_out.write(new_topic, new_msg, t, connection_header=conn_header) bag_in.close() bag_out.close()
def writepc2frombag2file(input_bagfile, pc2_topic, out_file_path,start_time=None, end_time=None, msg_count=None): try: output_file_name = get_output_file_name(input_bagfile) + '_xyz.txt' if out_file_path.endswith('/'): output_file = out_file_path + output_file_name else: output_file = out_file_path+'/' + output_file_name output_file_fh = open(output_file,'w') print('writing output_file at', output_file) # check start and end time condition if start_time is not None and end_time is not None: assert (end_time - start_time) > 0 , "end_time should be higher than start time" if msg_count is None: use_msg_count = False else: use_msg_count = True # msg_count is already static casted in arg parse, just checking again for sanity ad if function is re used individially msg_count = int(msg_count) assert msg_count > 0, "should have positive msg_count" input_bag = Bag(input_bagfile,'r') print('bag load success') count = 0; for topic, msg, ts in input_bag.read_messages(topics=[pc2_topic],start_time=start_time, end_time=end_time): output_file_fh.write('msg_timestamp: %f\n' % msg.header.stamp.to_sec()) output_file_fh.write('%d\n' %msg.width) for data_pts in pc2.read_points(msg, field_names=("x", "y", "z", "intensity"), skip_nans=True): output_file_fh.write('%f %f %f %d\n' % (data_pts[0], data_pts[1], data_pts[2], data_pts[3])) count +=1 print(count,msg_count) if use_msg_count and count >= msg_count: break input_bag.close() output_file_fh.close() except Exception as e_init: print(traceback.format_exc(e_init))
def on_enter(self, userdata): self._failed = False try: # Initialization # userdata self._bag_filename = userdata.bag_filename self._trajectories_command = userdata.trajectories_command # joint definitions l_arm_range = range(16, 23) r_arm_range = range(23, 30) atlasJointNames = [ 'back_bkz', 'back_bky', 'back_bkx', 'neck_ry', 'l_leg_hpz', 'l_leg_hpx', 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky', 'l_leg_akx', 'r_leg_hpz', 'r_leg_hpx', 'r_leg_hpy', 'r_leg_kny', 'r_leg_aky', 'r_leg_akx', 'l_arm_shz', 'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_wry', 'l_arm_wrx', 'l_arm_wry2', 'r_arm_shz', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_wry', 'r_arm_wrx', 'r_arm_wry2' ] joint_position_cmd = [0] * len(l_arm_range) # starting and stopping time of measurement data to be taken into account for cailbration time_start = 0 time_end = 0 time_tf = 0 # gravity vector in world frame g_world = np.resize(np.array([0, 0, -9.81]), (3, 1)) # take this number of data points for each pose. max_data_points_per_pose = 100 # rotation of the force torque sensor in world frame (quaternions) ft_rotation = [0, 0, 0, 0] # loop over all ft sensors to calibrate for chain in self._calibration_chain: tf_chain = ['/pelvis', 'ltorso', 'mtorso', 'utorso'] if chain == 'left_arm': joint_range = l_arm_range tfname = 'l_hand' tf_chain.extend([ 'l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm', 'l_lfarm', 'l_hand' ]) elif chain == 'right_arm': joint_range = r_arm_range tf_chain.extend([ 'r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm', 'r_lfarm', 'r_hand' ]) tfname = 'r_hand' else: Logger.logwarn( 'CalculateForceTorqueCalibration: Undefined chain %s', chain) # initialize transformations tf_data = [ ] # frame to frame transformation with numerical indexes from the tf_chain tf_data_cum = [ ] # world to frame transformation with numerical indexes from the tf_chain for i in range(len(tf_chain)): tf_data.append([0, 0, 0, 0]) tf_data_cum.append([0, 0, 0, 0]) # get number of poses from the commanded trajectories. 2 Points per Pose number_of_poses = len( self._trajectories_command[chain].points) / 2 # define information matrix for calibration InfMat = np.zeros( (6 * max_data_points_per_pose * number_of_poses, 10)) InfMat_i = 0 # Index in this Matrix # definie measurement vector for calibration MeasVec = np.zeros( (6 * max_data_points_per_pose * number_of_poses, 1)) # commanded joint trajectory for current chain # all commanded positions in this trajectory current_traj_cmd = self._trajectories_command[chain] # flag if the time interval was already set: timesetflag = False point_index_cmd = 0 # start with index 0 transformation_available = False # read time series from bag file # check in desired input trajectory, at which time a position is reached # take the data in a defined time period after the commanded new position bag_from_robot = Bag(os.path.expanduser( self._bag_filename)) # open bag file Logger.loginfo( 'CalculateForceTorqueCalibration: Calibrate %s from %s. Using %d different positions from trajectory command. Expecting 2 commanded positions per pose' % (chain, self._bag_filename, len(current_traj_cmd.points))) for topic, msg, t in bag_from_robot.read_messages(topics=[ '/flor/controller/atlas_state', '/tf', '/flor/controller/joint_command' ]): # Check if all desired poses have been reached if point_index_cmd > len(current_traj_cmd.points) - 1: break current_position_cmd = current_traj_cmd.points[ point_index_cmd].positions #################################################### # Check tf message: remember last transformation from world to ft sensor if topic == '/tf': # loop over all transformations inside this tf message for j in range(len(msg.transforms)): data = msg.transforms[j] tr = data.transform # current transformation header = data.header # check if frame matches on of the saved frames for i in range(len(tf_chain)): if data.child_frame_id == tf_chain[i]: tf_data[i] = np.array([ tr.rotation.x, tr.rotation.y, tr.rotation.z, tr.rotation.w ]) # calculate the newest transformation to the force torque sensor tf_data_cum[0] = tf_data[0] for i in range(1, len(tf_chain)): tf_data_cum[ i] = q = tf.transformations.quaternion_multiply( tf_data_cum[i - 1], tf_data[i]) if i == len(tf_chain) - 1: time_tf = msg.transforms[ 0].header.stamp.to_sec() # nothing to do with tf message # check if all transformations are available. (up to the last link) if np.any( tf_data_cum[-1] ): # real part of quaternion unequal to zero: data exists transformation_available = True continue else: time_msg = msg.header.stamp.to_sec() #################################################### # check if timestamp is interesting. Then check the if not (time_msg > time_start and time_msg < time_end) and not timesetflag: # the timestamp is not in evaluation interval. Look for reaching of the pose # record data (see below) if topic == '/flor/controller/joint_command': # get the commanded position for i in range(len(joint_range)): osrf_ndx = joint_range[i] joint_position_cmd[i] = msg.position[osrf_ndx] # Check if position command matches the currently expected commanded position pos_reached = True for i in range(len(joint_range)): if abs(joint_position_cmd[i] - current_position_cmd[i]) > 0.001: pos_reached = False break if pos_reached: # end time of the values regarded for calibration data: take the latest time possible for this pose time_end = msg.header.stamp.to_sec( ) + self._settlingtime # starting time for calibration: Take 100ms time_start = time_end - 0.1 data_points_for_this_pose = 0 timesetflag = True # take the next point next time. Each pose consists of two trajectory points (one for reaching, one for settling). # take the second one point_index_cmd = point_index_cmd + 2 if not pos_reached: # The commanded position has not been reached. Skip continue continue # continue here, because data aquisition is triggered by the time_start, time_end if time_msg > time_end: timesetflag = False # prepare for new evaluation interval #################################################### # check if enough datapoints for this pose have been collected if data_points_for_this_pose > max_data_points_per_pose: # already enough data points for this pose continue #################################################### # Check if message is atlas_state # IF this is the case, fill information matrix if topic != '/flor/controller/atlas_state': continue # Extract measured force and torque if chain == 'left_arm': FT = [ msg.l_hand.force.x, msg.l_hand.force.y, msg.l_hand.force.z, msg.l_hand.torque.x, msg.l_hand.torque.y, msg.l_hand.torque.z ] elif chain == 'right_arm': FT = [ msg.r_hand.force.x, msg.r_hand.force.y, msg.r_hand.force.z, msg.r_hand.torque.x, msg.r_hand.torque.y, msg.r_hand.torque.z ] # calculate gravitation vector if not transformation_available: Logger.logwarn( 'No tf messages available at time %1.4f.' % time_msg) continue R = tf.transformations.quaternion_matrix(tf_data_cum[-1]) g = np.dot((R[0:3, 0:3]).transpose(), g_world) gx = g[0] gy = g[1] gz = g[2] # fill information matrix for this data point. Source: [1], equ. (7) M = np.zeros((6, 10)) M[0, 0] = gx M[1, 0] = gy M[2, 0] = gz M[3, 2] = gz M[3, 3] = -gy M[4, 1] = -gz M[4, 3] = gx M[5, 1] = gy M[5, 2] = -gx M[0, 4] = 1.0 M[1, 5] = 1.0 M[2, 6] = 1.0 M[3, 7] = 1.0 M[4, 8] = 1.0 M[5, 9] = 1.0 # fill big information matrix and vector (stack small information matrizes) for i in range(6): for j in range(10): InfMat[InfMat_i * 6 + i, j] = M[i, j] MeasVec[InfMat_i * 6 + i, 0] = FT[i] InfMat_i = InfMat_i + 1 # increase index data_points_for_this_pose = data_points_for_this_pose + 1 # shorten big information matrix if InfMat_i < max_data_points_per_pose * number_of_poses: InfMat_calc = InfMat[0:(6 * InfMat_i) - 1, :] MeasVec_calc = MeasVec[0:(6 * InfMat_i - 1), :] else: InfMat_calc = InfMat MeasVec_calc = MeasVec # calculate calibration data if chain in self._static_calibration_data.keys( ): # calculate calibration with given static parameters # bring colums with first parameters on the other side of the equation if len(self._static_calibration_data[chain]) != 4: Logger.logwarn( "CalculateForceTorqueCalibration: Given static calibration data for %s has length %d. Required 4 entries." % (chain, len(k_fix))) self._failed = True return # convert physical parameters to identification parameters (mass, 1st moment) m = self._static_calibration_data[chain][0] if m == 0: mom_x = 0 mom_y = 0 mom_z = 0 elif m > 0: mom_x = self._static_calibration_data[chain][1] / m mom_y = self._static_calibration_data[chain][2] / m mom_z = self._static_calibration_data[chain][3] / m else: Logger.logwarn( "CalculateForceTorqueCalibration: Negative mass (%f) for calibration requested. Abort." % mass) self._failed = True return k_fix = np.resize(np.array([m, mom_x, mom_y, mom_z]), (4, 1)) Logger.loginfo( "CalculateForceTorqueCalibration:static calibration data for %s given: %s. Reduce equation system" % (chain, str(self._static_calibration_data[chain]))) MeasVec_calc_corr = np.subtract( np.array(MeasVec_calc), np.dot(InfMat_calc[:, 0:4], k_fix)) InfMat_calc_corr = InfMat_calc[:, 4: 10] # only the last 6 colums which correspond to the sensor offsets k = np.linalg.lstsq( InfMat_calc_corr, MeasVec_calc_corr)[0] # solve reduced equation system k_calibration = self._static_calibration_data[chain] k_calibration.extend(k) else: # calculate normally with all parameters unknown k = np.linalg.lstsq(InfMat_calc, MeasVec_calc)[0] # convert to physical parameters (first moment -> mass) if k[0] > 0: k_calibration = [ k[0], k[1] / k[0], k[2] / k[0], k[3] / k[0], k[4], k[5], k[6], k[7], k[8], k[9] ] else: Logger.loginfo( "CalculateForceTorqueCalibration:Calibration brought negative mass %f" % k[0]) k_calibration = [ 0.0, 0.0, 0.0, 0.0, k[4], k[5], k[6], k[7], k[8], k[9] ] Logger.loginfo( "CalculateForceTorqueCalibration:calibration data for %s" % chain) Logger.loginfo("CalculateForceTorqueCalibration:mass: %f" % float(k_calibration[0])) Logger.loginfo( "CalculateForceTorqueCalibration:center of mass: %f %f %f" % (k_calibration[1], k_calibration[2], k_calibration[3])) Logger.loginfo( "CalculateForceTorqueCalibration:F offset: %f %f %f" % (k_calibration[4], k_calibration[5], k_calibration[6])) Logger.loginfo( "CalculateForceTorqueCalibration:M offset: %f %f %f" % (k_calibration[7], k_calibration[8], k_calibration[9])) self._ft_calib_data[chain] = k_calibration bag_from_robot.close() userdata.ft_calib_data = self._ft_calib_data Logger.loginfo( 'CalculateForceTorqueCalibration:Calibration finished') self._done = True except Exception as e: Logger.logwarn( 'CalculateForceTorqueCalibration:Unable to calculate calibration:\n%s' % str(e)) self._failed = True
def on_enter(self, userdata): self._failed = False try: # Initialization # userdata self._bag_filename = userdata.bag_filename self._trajectories_command = userdata.trajectories_command # joint definitions l_arm_range = range(16,23); r_arm_range = range(23,30); atlasJointNames = [ 'back_bkz', 'back_bky', 'back_bkx', 'neck_ry', 'l_leg_hpz', 'l_leg_hpx', 'l_leg_hpy', 'l_leg_kny', 'l_leg_aky', 'l_leg_akx', 'r_leg_hpz', 'r_leg_hpx', 'r_leg_hpy', 'r_leg_kny', 'r_leg_aky', 'r_leg_akx', 'l_arm_shz', 'l_arm_shx', 'l_arm_ely', 'l_arm_elx', 'l_arm_wry', 'l_arm_wrx', 'l_arm_wry2', 'r_arm_shz', 'r_arm_shx', 'r_arm_ely', 'r_arm_elx', 'r_arm_wry', 'r_arm_wrx', 'r_arm_wry2'] joint_position_cmd = [0]*len(l_arm_range) # starting and stopping time of measurement data to be taken into account for cailbration time_start = 0 time_end = 0 time_tf = 0 # gravity vector in world frame g_world = np.resize(np.array([0,0,-9.81]),(3,1)) # take this number of data points for each pose. max_data_points_per_pose = 100 # rotation of the force torque sensor in world frame (quaternions) ft_rotation = [0,0,0,0] # loop over all ft sensors to calibrate for chain in self._calibration_chain: tf_chain=['/pelvis', 'ltorso', 'mtorso', 'utorso'] if chain == 'left_arm': joint_range = l_arm_range tfname = 'l_hand' tf_chain.extend(['l_clav', 'l_scap', 'l_uarm', 'l_larm', 'l_ufarm', 'l_lfarm', 'l_hand']) elif chain == 'right_arm': joint_range = r_arm_range tf_chain.extend(['r_clav', 'r_scap', 'r_uarm', 'r_larm', 'r_ufarm', 'r_lfarm', 'r_hand']) tfname = 'r_hand' else: Logger.logwarn('CalculateForceTorqueCalibration: Undefined chain %s', chain) # initialize transformations tf_data = [] # frame to frame transformation with numerical indexes from the tf_chain tf_data_cum = [] # world to frame transformation with numerical indexes from the tf_chain for i in range(len(tf_chain)): tf_data.append([0,0,0,0]) tf_data_cum.append([0,0,0,0]) # get number of poses from the commanded trajectories. 2 Points per Pose number_of_poses = len(self._trajectories_command[chain].points) / 2 # define information matrix for calibration InfMat = np.zeros((6*max_data_points_per_pose*number_of_poses, 10)) InfMat_i = 0 # Index in this Matrix # definie measurement vector for calibration MeasVec = np.zeros((6*max_data_points_per_pose*number_of_poses, 1)) # commanded joint trajectory for current chain # all commanded positions in this trajectory current_traj_cmd = self._trajectories_command[chain] # flag if the time interval was already set: timesetflag = False point_index_cmd = 0 # start with index 0 transformation_available = False # read time series from bag file # check in desired input trajectory, at which time a position is reached # take the data in a defined time period after the commanded new position bag_from_robot = Bag(os.path.expanduser(self._bag_filename)) # open bag file Logger.loginfo('CalculateForceTorqueCalibration: Calibrate %s from %s. Using %d different positions from trajectory command. Expecting 2 commanded positions per pose' % (chain, self._bag_filename, len(current_traj_cmd.points)) ) for topic, msg, t in bag_from_robot.read_messages(topics=['/flor/controller/atlas_state', '/tf', '/flor/controller/joint_command']): # Check if all desired poses have been reached if point_index_cmd > len(current_traj_cmd.points)-1: break current_position_cmd = current_traj_cmd.points[point_index_cmd].positions #################################################### # Check tf message: remember last transformation from world to ft sensor if topic == '/tf': # loop over all transformations inside this tf message for j in range(len(msg.transforms)): data = msg.transforms[j] tr = data.transform # current transformation header = data.header # check if frame matches on of the saved frames for i in range(len(tf_chain)): if data.child_frame_id == tf_chain[i]: tf_data[i] = np.array([tr.rotation.x, tr.rotation.y, tr.rotation.z, tr.rotation.w]) # calculate the newest transformation to the force torque sensor tf_data_cum[0] = tf_data[0] for i in range(1,len(tf_chain)): tf_data_cum[i] = q = tf.transformations.quaternion_multiply(tf_data_cum[i-1], tf_data[i]) if i == len(tf_chain)-1: time_tf = msg.transforms[0].header.stamp.to_sec() # nothing to do with tf message # check if all transformations are available. (up to the last link) if np.any(tf_data_cum[-1]): # real part of quaternion unequal to zero: data exists transformation_available = True continue else: time_msg = msg.header.stamp.to_sec() #################################################### # check if timestamp is interesting. Then check the if not (time_msg > time_start and time_msg < time_end) and not timesetflag: # the timestamp is not in evaluation interval. Look for reaching of the pose # record data (see below) if topic == '/flor/controller/joint_command': # get the commanded position for i in range(len(joint_range)): osrf_ndx = joint_range[i] joint_position_cmd[i] = msg.position[osrf_ndx] # Check if position command matches the currently expected commanded position pos_reached = True for i in range(len(joint_range)): if abs(joint_position_cmd[i]-current_position_cmd[i]) > 0.001: pos_reached = False break if pos_reached: # end time of the values regarded for calibration data: take the latest time possible for this pose time_end = msg.header.stamp.to_sec() + self._settlingtime # starting time for calibration: Take 100ms time_start = time_end - 0.1 data_points_for_this_pose = 0 timesetflag = True # take the next point next time. Each pose consists of two trajectory points (one for reaching, one for settling). # take the second one point_index_cmd = point_index_cmd + 2 if not pos_reached: # The commanded position has not been reached. Skip continue continue # continue here, because data aquisition is triggered by the time_start, time_end if time_msg > time_end: timesetflag = False # prepare for new evaluation interval #################################################### # check if enough datapoints for this pose have been collected if data_points_for_this_pose > max_data_points_per_pose: # already enough data points for this pose continue #################################################### # Check if message is atlas_state # IF this is the case, fill information matrix if topic != '/flor/controller/atlas_state': continue # Extract measured force and torque if chain == 'left_arm': FT = [msg.l_hand.force.x, msg.l_hand.force.y, msg.l_hand.force.z, msg.l_hand.torque.x, msg.l_hand.torque.y, msg.l_hand.torque.z ] elif chain == 'right_arm': FT = [msg.r_hand.force.x, msg.r_hand.force.y, msg.r_hand.force.z, msg.r_hand.torque.x, msg.r_hand.torque.y, msg.r_hand.torque.z ] # calculate gravitation vector if not transformation_available: Logger.logwarn('No tf messages available at time %1.4f.' % time_msg) continue R = tf.transformations.quaternion_matrix(tf_data_cum[-1]) g = np.dot((R[0:3,0:3]).transpose(), g_world) gx = g[0] gy = g[1] gz = g[2] # fill information matrix for this data point. Source: [1], equ. (7) M = np.zeros((6, 10)) M[0,0] = gx M[1,0] = gy M[2,0] = gz M[3,2] = gz M[3,3] = -gy M[4,1] = -gz M[4,3] = gx M[5,1] = gy M[5,2] = -gx M[0,4] = 1.0 M[1,5] = 1.0 M[2,6] = 1.0 M[3,7] = 1.0 M[4,8] = 1.0 M[5,9] = 1.0 # fill big information matrix and vector (stack small information matrizes) for i in range(6): for j in range(10): InfMat[InfMat_i*6+i,j] = M[i,j] MeasVec[InfMat_i*6+i,0] = FT[i] InfMat_i = InfMat_i + 1 # increase index data_points_for_this_pose = data_points_for_this_pose + 1 # shorten big information matrix if InfMat_i < max_data_points_per_pose*number_of_poses: InfMat_calc = InfMat[0:(6*InfMat_i)-1,:] MeasVec_calc = MeasVec[0:(6*InfMat_i-1),:] else: InfMat_calc = InfMat MeasVec_calc = MeasVec # calculate calibration data if chain in self._static_calibration_data.keys(): # calculate calibration with given static parameters # bring colums with first parameters on the other side of the equation if len(self._static_calibration_data[chain]) != 4: Logger.logwarn( "CalculateForceTorqueCalibration: Given static calibration data for %s has length %d. Required 4 entries." % (chain, len(k_fix)) ) self._failed = True return # convert physical parameters to identification parameters (mass, 1st moment) m = self._static_calibration_data[chain][0] if m == 0: mom_x = 0 mom_y = 0 mom_z = 0 elif m > 0: mom_x = self._static_calibration_data[chain][1]/m mom_y = self._static_calibration_data[chain][2]/m mom_z = self._static_calibration_data[chain][3]/m else: Logger.logwarn( "CalculateForceTorqueCalibration: Negative mass (%f) for calibration requested. Abort." % mass ) self._failed = True return k_fix = np.resize(np.array([m, mom_x, mom_y, mom_z]),(4,1)) Logger.loginfo( "CalculateForceTorqueCalibration:static calibration data for %s given: %s. Reduce equation system" % (chain, str(self._static_calibration_data[chain])) ) MeasVec_calc_corr = np.subtract(np.array(MeasVec_calc), np.dot(InfMat_calc[:,0:4], k_fix)) InfMat_calc_corr = InfMat_calc[:,4:10] # only the last 6 colums which correspond to the sensor offsets k = np.linalg.lstsq(InfMat_calc_corr, MeasVec_calc_corr)[0] # solve reduced equation system k_calibration = self._static_calibration_data[chain] k_calibration.extend(k) else: # calculate normally with all parameters unknown k = np.linalg.lstsq(InfMat_calc, MeasVec_calc)[0] # convert to physical parameters (first moment -> mass) if k[0] > 0: k_calibration = [k[0], k[1]/k[0], k[2]/k[0], k[3]/k[0], k[4], k[5], k[6], k[7], k[8], k[9]] else: Logger.loginfo("CalculateForceTorqueCalibration:Calibration brought negative mass %f" % k[0]) k_calibration = [0.0, 0.0, 0.0, 0.0, k[4], k[5], k[6], k[7], k[8], k[9]] Logger.loginfo("CalculateForceTorqueCalibration:calibration data for %s" % chain) Logger.loginfo("CalculateForceTorqueCalibration:mass: %f" % float(k_calibration[0])) Logger.loginfo("CalculateForceTorqueCalibration:center of mass: %f %f %f" % (k_calibration[1], k_calibration[2], k_calibration[3])) Logger.loginfo("CalculateForceTorqueCalibration:F offset: %f %f %f" % (k_calibration[4], k_calibration[5], k_calibration[6])) Logger.loginfo("CalculateForceTorqueCalibration:M offset: %f %f %f" % (k_calibration[7], k_calibration[8], k_calibration[9])) self._ft_calib_data[chain] = k_calibration bag_from_robot.close() userdata.ft_calib_data = self._ft_calib_data Logger.loginfo('CalculateForceTorqueCalibration:Calibration finished') self._done = True except Exception as e: Logger.logwarn('CalculateForceTorqueCalibration:Unable to calculate calibration:\n%s' % str(e)) self._failed = True
from rosbag import Bag from std_msgs.msg import String bag = Bag("objective.bag", "w") bag.write("/liability", String("0xaaEc8f06cd803Daf8807C5aB10660b95d385a0dC")) bag.write("/log_hash", String("QmRTsJ4cT3Qax8bfAepTEne1rbFiPkgUApemtWCj9FhEgd")) bag.close()
def create_ground_truth(path, time_offset=0, bagfile='.logs/*.bag', topic='/StereoTUM/estimated_transform', file=''): from rosbag import Bag from geometry_msgs.msg import TransformStamped from glob import glob as findfiles if not file: file = p.join(path, 'data', 'ground_truth.csv') first = None msgs = [] timefiles = findfiles(p.join(path, 'params', 'time.yaml')) if not timefiles: raise ValueError('Can\'t find %s/params/time.yaml' % path) return seq_stamps = (None, None) with open(timefiles[0]) as stream: time = yaml.load(stream) seq_stamps = (time['time']['start'], time['time']['end']) print('Using time offset: %fs' % time_offset) a = p.join(path, bagfile) b = findfiles(a) if not b: raise ValueError('Can\'t find the bag file %s' % a) return bag = Bag(b[0]) print('Found ground truth file at: %s' % b[0]) print('Will listen to topic %s' % topic) for bagtopic, msg, t in bag.read_messages(): if bagtopic != topic: continue now = t.to_sec() + time_offset if now < seq_stamps[0]: continue # before recording started if first is None: first = now if now > seq_stamps[1]: continue # after recording finished stamp = now - first # Check if any message with the same timestamp has already been added... if any(item.get('time', None) == stamp for item in msgs): continue # If not, add the message to the list of ground truths msgs.append({ 'time': stamp, 'trans': msg.transform.translation, 'orien': msg.transform.rotation }) bag.close() print('Noticed %d messages, will now sort them by time...' % len(msgs)) msgs = sorted(msgs, key=lambda x: x['time']) with open(file, 'w') as f: print('Creating new file %s' % file) f.write( 'Timestamp [s]\tTranslation X [m]\tTranslation Y [m]\tTranslation Z [m]\tOrientation W\tOrientation X\tOrientation Y\tOrientation Z\n' ) for msg in msgs: f.write('%.6f\t' % msg['time']) f.write('%6.4f\t' % msg['trans'].x) f.write('%6.4f\t' % msg['trans'].y) f.write('%6.4f\t' % msg['trans'].z) f.write('%6.4f\t' % msg['orien'].w) f.write('%6.4f\t' % msg['orien'].x) f.write('%6.4f\t' % msg['orien'].y) f.write('%6.4f' % msg['orien'].z) f.write('\n') # remove last newline from file f.seek(-1, os.SEEK_END) f.truncate()
def parse_bag(bag_file_path): bagfile = Bag(bag_file_path) msgs_topics = [ "/drone/pose", "/gazebo/ground_truth", "/mavros/local_position/odom", "/mavros/px4flow/ground_distance" ] msgs_drone_pose = { "y": [], "z": [], "x": [], "roll": [], "pitch": [], "yaw": [], "time": [], "label": "EKF-SLAM" } msgs_ground_truth = { "y": [], "z": [], "x": [], "roll": [], "pitch": [], "yaw": [], "time": [], "label": "Ground Truth" } msgs_mavros_odom = { "y": [], "z": [], "x": [], "roll": [], "pitch": [], "yaw": [], "time": [], "label": "MAVROS" } msgs_mavros_laser = {"range": [], "time": [], "label": "Laser Range"} for topic, message, time in bagfile.read_messages(msgs_topics): if topic == msgs_topics[0]: msgs_drone_pose["x"].append(message.pose.pose.position.x) msgs_drone_pose["y"].append(message.pose.pose.position.y) msgs_drone_pose["z"].append(message.pose.pose.position.z) quat = message.pose.pose.orientation euler = quat2euler([quat.x, quat.y, quat.z, quat.w]) msgs_drone_pose["roll"].append(euler[0]) msgs_drone_pose["pitch"].append(euler[1]) msgs_drone_pose["yaw"].append(euler[2]) msgs_drone_pose["time"].append(time.to_sec()) if topic == msgs_topics[1]: msgs_ground_truth["x"].append(message.pose.pose.position.x) msgs_ground_truth["y"].append(message.pose.pose.position.y) msgs_ground_truth["z"].append(message.pose.pose.position.z) quat = message.pose.pose.orientation euler = quat2euler([quat.x, quat.y, quat.z, quat.w]) msgs_ground_truth["roll"].append(euler[0]) msgs_ground_truth["pitch"].append(euler[1]) msgs_ground_truth["yaw"].append(euler[2]) msgs_ground_truth["time"].append(time.to_sec()) if topic == msgs_topics[2]: msgs_mavros_odom["x"].append(message.pose.pose.position.x - 3) msgs_mavros_odom["y"].append(message.pose.pose.position.y) msgs_mavros_odom["z"].append(message.pose.pose.position.z) quat = message.pose.pose.orientation euler = quat2euler([quat.x, quat.y, quat.z, quat.w]) msgs_mavros_odom["roll"].append(euler[0]) msgs_mavros_odom["pitch"].append(euler[1]) msgs_mavros_odom["yaw"].append(euler[2]) msgs_mavros_odom["time"].append(time.to_sec()) if topic == msgs_topics[3]: msgs_mavros_laser["range"].append(message.range) msgs_mavros_laser["time"].append(time.to_sec()) bagfile.close() return { "ground_truth": msgs_ground_truth, "mavros_odom": msgs_mavros_odom, "mavros_laser": msgs_mavros_laser, "ekf2": msgs_drone_pose }