def odom_to_speed(odom): header = Header() header.stamp = rospy.Time.from_sec(time.time()) dimensions = [ MetricDimension(name='robot_id', value='Turtlebot3'), MetricDimension(name='category', value='RobotOperations'), ] linear_speed = MetricData( header=header, metric_name='linear_speed', unit=MetricData.UNIT_NONE, value=odom.twist.twist.linear.x, time_stamp=rospy.Time.from_sec(time.time()), dimensions=dimensions, ) angular_speed = MetricData( header=header, metric_name='angular_speed', unit=MetricData.UNIT_NONE, value=odom.twist.twist.angular.z, time_stamp=rospy.Time.from_sec(time.time()), dimensions=dimensions, ) return MetricList([linear_speed, angular_speed])
def odom_to_speed(self, odom): header = Header() timestamp = self.get_clock().now().to_msg() header.stamp = timestamp dimensions = [ MetricDimension(name="robot_id", value="Turtlebot3"), MetricDimension(name="category", value="RobotOperations") ] linear_speed = MetricData(header=header, metric_name="linear_speed", unit=MetricData.UNIT_NONE, value=odom.twist.twist.linear.x, time_stamp=timestamp, dimensions=dimensions) angular_speed = MetricData(header=header, metric_name="angular_speed", unit=MetricData.UNIT_NONE, value=odom.twist.twist.angular.z, time_stamp=timestamp, dimensions=dimensions) return MetricList(metrics=[linear_speed, angular_speed])
def report_metric(self, msg): if not msg.poses: rospy.logdebug('Path empty, not calculating distance') return distance = self.calc_path_distance(msg) rospy.logdebug('Distance to goal: %s', distance) header = Header() header.stamp = rospy.Time.from_sec(time.time()) dimensions = [ MetricDimension(name='robot_id', value='Turtlebot3'), MetricDimension(name='category', value='RobotOperations'), ] metric = MetricData( header=header, metric_name='distance_to_goal', unit=MetricData.UNIT_NONE, value=distance, time_stamp=rospy.Time.from_sec(time.time()), dimensions=dimensions, ) self.metrics_pub.publish(MetricList([metric]))
def report_metric(self, msg): filtered_scan = self.filter_scan(msg) if not filtered_scan: rospy.loginfo( 'No obstacles with scan range (%s,%s)', msg.range_min, msg.range_max ) return min_distance = min(filtered_scan) rospy.loginfo('Nearest obstacle: %s', min_distance) header = Header() header.stamp = rospy.Time.from_sec(time.time()) dimensions = [ MetricDimension(name='robot_id', value='Turtlebot3'), MetricDimension(name='category', value='RobotOperations'), ] metric = MetricData( header=header, metric_name='nearest_obstacle_distance', unit=MetricData.UNIT_NONE, value=min_distance, time_stamp=rospy.Time.from_sec(time.time()), dimensions=dimensions, ) self.metrics_pub.publish(MetricList([metric]))
def report_metric(self, msg): if not msg.poses: self.get_logger().debug('Path empty, not calculating distance') return distance = self.calc_path_distance(msg) self.get_logger().debug(f"Distance to goal: {distance}") timestamp = self.get_clock().now().to_msg() header = Header() header.stamp = timestamp dimensions = [ MetricDimension(name="robot_id", value="Turtlebot3"), MetricDimension(name="category", value="RobotOperations") ] metric = MetricData(header=header, metric_name="distance_to_goal", unit=MetricData.UNIT_NONE, value=distance, time_stamp=timestamp, dimensions=dimensions) self.metrics_pub.publish(MetricList(metrics=[metric]))
def report_metric(self, msg): filtered_scan = self.filter_scan(msg) if not filtered_scan: self.get_logger().info( f"No obstacles with scan range ({msg.range_min}, {msg.range_max})" ) return min_distance = min(filtered_scan) self.get_logger().info(f"Nearest obstacle: {min_distance}") timestamp = self.get_clock().now().to_msg() header = Header() header.stamp = timestamp dimensions = [ MetricDimension(name="robot_id", value="Turtlebot3"), MetricDimension(name="category", value="RobotOperations") ] metric = MetricData(header=header, metric_name="nearest_obstacle_distance", unit=MetricData.UNIT_NONE, value=min_distance, time_stamp=timestamp, dimensions=dimensions) self.metrics_pub.publish(MetricList(metrics=[metric]))