def _set_initial_localization_waypoint(self, *args): """Trigger localization to a waypoint.""" # Take the first argument as the localization waypoint. if len(args) < 1: # If no waypoint id is given as input, then return without initializing. print("No waypoint specified to initialize to.") return destination_waypoint = graph_nav_util.find_unique_waypoint_id( args[0][0], self._current_graph, self._current_annotation_name_to_wp_id) if not destination_waypoint: # Failed to find the unique waypoint id. return robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( robot_state.kinematic_state.transforms_snapshot).to_proto() # Create an initial localization to the specified waypoint as the identity. localization = nav_pb2.Localization() localization.waypoint_id = destination_waypoint localization.waypoint_tform_body.rotation.w = 1.0 self._graph_nav_client.set_localization( initial_guess_localization=localization, # It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m). max_distance=0.2, max_yaw=20.0 * math.pi / 180.0, fiducial_init=graph_nav_pb2.SetLocalizationRequest. FIDUCIAL_INIT_NO_FIDUCIAL, ko_tform_body=current_odom_tform_body)
def GetOdomFromState(state, spot_wrapper, use_vision=True): """Maps odometry data from robot state proto to ROS Odometry message Args: data: Robot State proto spot_wrapper: A SpotWrapper object Returns: Odometry message """ odom_msg = Odometry() local_time = spot_wrapper.robotToLocalTime( state.kinematic_state.acquisition_timestamp) odom_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos) if use_vision == True: odom_msg.header.frame_id = 'vision' tform_body = get_vision_tform_body( state.kinematic_state.transforms_snapshot) else: odom_msg.header.frame_id = 'odom' tform_body = get_odom_tform_body( state.kinematic_state.transforms_snapshot) odom_msg.child_frame_id = 'body' pose_odom_msg = PoseWithCovariance() pose_odom_msg.pose.position.x = tform_body.position.x pose_odom_msg.pose.position.y = tform_body.position.y pose_odom_msg.pose.position.z = tform_body.position.z pose_odom_msg.pose.orientation.x = tform_body.rotation.x pose_odom_msg.pose.orientation.y = tform_body.rotation.y pose_odom_msg.pose.orientation.z = tform_body.rotation.z pose_odom_msg.pose.orientation.w = tform_body.rotation.w odom_msg.pose = pose_odom_msg twist_odom_msg = GetOdomTwistFromState(state, spot_wrapper).twist odom_msg.twist = twist_odom_msg return odom_msg
def _get_localization_state(self, *args): """Get the current localization and state of the robot.""" state = self._graph_nav_client.get_localization_state() print('Got localization: \n%s' % str(state.localization)) odom_tform_body = get_odom_tform_body( state.robot_kinematics.transforms_snapshot) print('Got robot state in odom frame: \n%s' % str(odom_tform_body))
def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name='odom'): """Send a trajectory motion command to the robot. Args: goal_x: Position X coordinate in meters goal_y: Position Y coordinate in meters goal_heading: Pose heading in radians cmd_duration: Time-to-live for the command in seconds. frame_name: frame_name to be used to calc the target position. 'odom' or 'vision' """ self._at_goal = False self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration if frame_name == 'vision': vision_tform_body = frame_helpers.get_vision_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) vision_tform_goal = vision_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.trajectory_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=frame_helpers.VISION_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) elif frame_name == 'odom': odom_tform_body = frame_helpers.get_odom_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) odom_tform_goal = odom_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.trajectory_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=frame_helpers.ODOM_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) else: raise ValueError('frame_name must be \'vision\' or \'odom\'') if response[0]: self._last_trajectory_command = response[2] return response[0], response[1]
def _set_initial_localization_fiducial(self, *args): """Trigger localization when near a fiducial.""" robot_state = self._robot_state_client.get_robot_state() current_odom_tform_body = get_odom_tform_body( robot_state.kinematic_state.transforms_snapshot).to_proto() # Create an empty instance for initial localization since we are asking it to localize # based on the nearest fiducial. localization = nav_pb2.Localization() self._graph_nav_client.set_localization(initial_guess_localization=localization, ko_tform_body=current_odom_tform_body)
def main(argv): # The last argument should be the IP address of the robot. The app will use the directory to find # the velodyne and start getting data from it. parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) options = parser.parse_args(argv) sdk = bosdyn.client.create_standard_sdk('VelodyneClient') robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) robot.sync_with_directory() _point_cloud_client = robot.ensure_client('velodyne-point-cloud') _robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) _point_cloud_task = AsyncPointCloud(_point_cloud_client) _robot_state_task = AsyncRobotState(_robot_state_client) _task_list = [_point_cloud_task, _robot_state_task] _async_tasks = AsyncTasks(_task_list) print('Connected.') update_thread = threading.Thread(target=_update_thread, args=[_async_tasks]) update_thread.daemon = True update_thread.start() # Wait for the first responses. while any(task.proto is None for task in _task_list): time.sleep(0.1) fig = plt.figure() body_tform_butt = SE3Pose(-0.5, 0, 0, Quat()) body_tform_head = SE3Pose(0.5, 0, 0, Quat()) # Plot the point cloud as an animation. ax = fig.add_subplot(111, projection='3d') aggregate_data = collections.deque(maxlen=5) while True: if _point_cloud_task.proto[0].point_cloud: data = np.fromstring(_point_cloud_task.proto[0].point_cloud.data, dtype=np.float32) aggregate_data.append(data) plot_data = np.concatenate(aggregate_data) ax.clear() ax.set_xlabel('X (m)') ax.set_ylabel('Y (m)') ax.set_zlabel('Z (m)') # Draw robot position and orientation on plot if _robot_state_task.proto: odom_tform_body = get_odom_tform_body( _robot_state_task.proto.kinematic_state.transforms_snapshot ).to_proto() helper_se3 = SE3Pose.from_obj(odom_tform_body) odom_tform_butt = helper_se3.mult(body_tform_butt) odom_tform_head = helper_se3.mult(body_tform_head) ax.plot([odom_tform_butt.x], [odom_tform_butt.y], [odom_tform_butt.z], 'o', color=SPOT_YELLOW, markersize=7, label='robot_butt') ax.plot([odom_tform_head.x], [odom_tform_head.y], [odom_tform_head.z], 'o', color=SPOT_YELLOW, markersize=7, label='robot_head') ax.text(odom_tform_butt.x, odom_tform_butt.y, odom_tform_butt.z, 'Spot Butt', size=TEXT_SIZE, zorder=1, color='k') ax.text(odom_tform_head.x, odom_tform_head.y, odom_tform_head.z, 'Spot Head', size=TEXT_SIZE, zorder=1, color='k') ax.plot([odom_tform_butt.x, odom_tform_head.x], [odom_tform_butt.y, odom_tform_head.y], zs=[odom_tform_butt.z, odom_tform_head.z], linewidth=6, color=SPOT_YELLOW) # Plot point cloud data ax.plot(plot_data[0::3], plot_data[1::3], plot_data[2::3], '.') set_axes_equal(ax) plt.draw() plt.pause(0.016) if window_closed(ax): sys.exit(0)
def main(): import argparse parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) options = parser.parse_args() # Create robot object. sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster') sdk.load_app_token(options.app_token) robot = sdk.create_robot(options.hostname) robot.authenticate(options.username, options.password) # Check that an estop is connected with the robot so that the robot commands can be executed. verify_estop(robot) # Create the lease client. lease_client = robot.ensure_client(LeaseClient.default_service_name) lease = lease_client.acquire() robot.time_sync.wait_for_sync() lk = bosdyn.client.lease.LeaseKeepAlive(lease_client) # Setup clients for the robot state and robot command services. robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) robot_command_client = robot.ensure_client( RobotCommandClient.default_service_name) # Power on the robot and stand it up. robot.power_on() blocking_stand(robot_command_client) # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. vision_tform_body = get_vision_tform_body( robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) # We want to command a trajectory to go forward one meter in the x-direction of the body. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter forward in the x-axis of the body. # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the # rotation of the body at the goal to match the rotation of the body currently, so we do not need # to transform the rotation. body_tform_goal = math_helpers.SE3Pose(x=1, y=0, z=0, rot=math_helpers.Quat()) # We can then transform this transform to get the goal position relative to the vision frame. vision_tform_goal = vision_tform_body * body_tform_goal # Command the robot to go to the goal point in the vision frame. The command will stop at the new # position in the vision frame. robot_cmd = RobotCommandBuilder.trajectory_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=VISION_FRAME_NAME) end_time = 2.0 robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) # Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body, # which describes the robot body's position in the odom frame. odom_tform_body = get_odom_tform_body(robot_state_client.get_robot_state(). kinematic_state.transforms_snapshot) # We want to command a trajectory to go backwards one meter and to the left one meter. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value) # in the y-axis of the body. body_tform_goal = math_helpers.SE3Pose(x=-1, y=1, z=0, rot=math_helpers.Quat()) # We can then transform this transform to get the goal position relative to the odom frame. odom_tform_goal = odom_tform_body * body_tform_goal # Command the robot to go to the goal point in the odom frame. The command will stop at the new # position in the odom frame. robot_cmd = RobotCommandBuilder.trajectory_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=ODOM_FRAME_NAME) end_time = 5.0 robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + end_time) time.sleep(end_time) return True
def get_robot_state(self): ''' Returns tuple of kinematic_state, robot_state kinematic_state: timestamp joint_states [] ko_tform_body body_twist_rt_ko ground_plane_rt_ko vo_tform_body robot_state: power_state battery_states[] comms_states[] system_fault_state estop_states[] behavior_fault_state ''' robot_state = self.robot_state_client.get_robot_state() rs_msg = spot_ros_msgs.msg.RobotState() ''' PowerState conversion ''' rs_msg.power_state.header.stamp.secs = robot_state.power_state.timestamp.seconds rs_msg.power_state.header.stamp.nsecs = robot_state.power_state.timestamp.nanos rs_msg.power_state.motor_power_state = robot_state.power_state.motor_power_state #[enum] rs_msg.power_state.shore_power_state = robot_state.power_state.shore_power_state #[enum] rs_msg.power_state.locomotion_charge_percentage = robot_state.power_state.locomotion_charge_percentage.value #[google.protobuf.DoubleValue] rs_msg.power_state.locomotion_estimated_runtime.secs = robot_state.power_state.locomotion_estimated_runtime.seconds #[google.protobuf.Duration] ''' BatteryState conversion [repeated field] ''' for battery_state in robot_state.battery_states: battery_state_msg = sensor_msgs.msg.BatteryState() header = std_msgs.msg.Header() header.stamp.secs = battery_state.timestamp.seconds header.stamp.nsecs = battery_state.timestamp.nanos header.frame_id = battery_state.identifier #[string] battery_state_msg.header = header battery_state_msg.percentage = battery_state.charge_percentage.value/100 #[double] # NOTE: Using battery_state_msg.charge as the estimated runtime in sec battery_state_msg.charge = battery_state.estimated_runtime.seconds #[google.protobuf.Duration] battery_state_msg.current = battery_state.current.value #[DoubleValue] battery_state_msg.voltage = battery_state.voltage.value #[DoubleValue] # NOTE: Ignoring battery_state.temperatures for now; no field in BatteryState maps directly to it battery_state_msg.power_supply_status = battery_state.status #[enum] rs_msg.battery_states.append(battery_state_msg) ''' CommsState conversion [repeated field] ''' for comms_state in robot_state.comms_states: comms_state_msg = spot_ros_msgs.msg.CommsState() comms_state_msg.header.stamp.secs = comms_state.timestamp.seconds #[google.protobuf.Timestamp] comms_state_msg.header.stamp.nsecs = comms_state.timestamp.nanos #[google.protobuf.Timestamp] comms_state_msg.wifi_mode = comms_state.wifi_state.current_mode #[enum] Note: wifi_state is oneof comms_state_msg.essid = comms_state.wifi_state.essid #[string] rs_msg.comms_states.append(comms_state_msg) ''' SystemFaultState conversion ''' ### faults is Repeated ### for fault in robot_state.system_fault_state.faults: system_fault_msg = spot_ros_msgs.msg.SystemFault() system_fault_msg.header.frame_id = fault.name #[string] system_fault_msg.header.stamp.secs = fault.onset_timestamp.seconds #[google.protobuf.Timestamp] system_fault_msg.header.stamp.nsecs = fault.onset_timestamp.nanos #[google.protobuf.Timestamp] system_fault_msg.duration.secs = fault.duration.seconds #[google.protobuf.Duration] system_fault_msg.duration.nsecs = fault.duration.nanos #[google.protobuf.Duration] system_fault_msg.code = fault.code #[int32] system_fault_msg.uid = fault.uid #[uint64] system_fault_msg.error_message = fault.error_message #[string] system_fault_msg.attributes = fault.attributes #[repeated-string] system_fault_msg.severity = fault.severity #[enum] rs_msg.system_fault_state.faults.append(system_fault_msg) ### historical_faults is Repeated ### for historical_fault in robot_state.system_fault_state.faults: system_fault_msg = spot_ros_msgs.msg.SystemFault() system_fault_msg.header.frame_id = historical_fault.name #[string] system_fault_msg.header.stamp.secs = historical_fault.onset_timestamp.seconds #[google.protobuf.Timestamp] system_fault_msg.header.stamp.nsecs = historical_fault.onset_timestamp.nanos #[google.protobuf.Timestamp] system_fault_msg.duration.secs = historical_fault.duration.seconds #[google.protobuf.Duration] system_fault_msg.duration.nsecs = historical_fault.duration.nanos #[google.protobuf.Duration] system_fault_msg.code = historical_fault.code #[int32] system_fault_msg.uid = historical_fault.uid #[uint64] system_fault_msg.error_message = historical_fault.error_message #[string] system_fault_msg.attributes = historical_fault.attributes #[repeated-string] system_fault_msg.severity = historical_fault.severity #[enum] rs_msg.system_fault_state.historical_faults.append(system_fault_msg) #[map<string,enum>] if robot_state.system_fault_state.aggregated: for key, value in robot_state.system_fault_state.aggregated.items(): kv = diagnostic_msgs.msg.KeyValue() kv.key = key kv.value = value rs_msg.system_fault_state.aggregated.append(kv) ''' EStopState conversion [repeated field] ''' for estop_state in robot_state.estop_states: estop_msg = spot_ros_msgs.msg.EStopState() estop_msg.header.stamp.secs = estop_state.timestamp.seconds #[google.protobuf.Timestamp] estop_msg.header.stamp.nsecs = estop_state.timestamp.nanos #[google.protobuf.Timestamp] estop_msg.header.frame_id = estop_state.name #[string] estop_msg.type = estop_state.type #[enum] estop_msg.state = estop_state.state #[enum] estop_msg.state_description = estop_state.state_description #[string] rs_msg.estop_states.append(estop_msg) ''' KinematicState conversion ''' ks_msg = spot_ros_msgs.msg.KinematicState() ks_msg.header.stamp.secs = robot_state.kinematic_state.acquisition_timestamp.seconds ks_msg.header.stamp.nsecs = robot_state.kinematic_state.acquisition_timestamp.nanos ### joint_states is repeated ### js = sensor_msgs.msg.JointState() js.header.stamp = ks_msg.header.stamp for joint_state in robot_state.kinematic_state.joint_states: js.name.append(joint_state.name) # [string] js.position.append(joint_state.position.value) # Note: angle in rad js.velocity.append(joint_state.velocity.value) # Note: ang vel # NOTE: ang accel. JointState doesn't have accel. Ignoring joint_state.acceleration for now. js.effort.append(joint_state.load.value) # Note: Torque in N-m ks_msg.joint_states = js # SE3Pose representing transform of Spot's Body frame relative to the inertial Vision frame vision_tform_body = get_vision_tform_body(robot_state.kinematic_state.transforms_snapshot) ks_msg.vision_tform_body.translation.x = vision_tform_body.x ks_msg.vision_tform_body.translation.y = vision_tform_body.y ks_msg.vision_tform_body.translation.z = vision_tform_body.z ks_msg.vision_tform_body.rotation.x = vision_tform_body.rot.x ks_msg.vision_tform_body.rotation.y = vision_tform_body.rot.y ks_msg.vision_tform_body.rotation.z = vision_tform_body.rot.z ks_msg.vision_tform_body.rotation.w = vision_tform_body.rot.w # odom_tform_body: SE3Pose representing transform of Spot's Body frame relative to the odometry frame odom_tform_body = get_odom_tform_body(robot_state.kinematic_state.transforms_snapshot) ks_msg.odom_tform_body.translation.x = odom_tform_body.x ks_msg.odom_tform_body.translation.y = odom_tform_body.y ks_msg.odom_tform_body.translation.z = odom_tform_body.z ks_msg.odom_tform_body.rotation.x = odom_tform_body.rot.x ks_msg.odom_tform_body.rotation.y = odom_tform_body.rot.y ks_msg.odom_tform_body.rotation.z = odom_tform_body.rot.z ks_msg.odom_tform_body.rotation.w = odom_tform_body.rot.w ''' velocity_of_body_in_vision ''' ks_msg.velocity_of_body_in_vision.linear.x = robot_state.kinematic_state.velocity_of_body_in_vision.linear.x ks_msg.velocity_of_body_in_vision.linear.y = robot_state.kinematic_state.velocity_of_body_in_vision.linear.y ks_msg.velocity_of_body_in_vision.linear.z = robot_state.kinematic_state.velocity_of_body_in_vision.linear.z ks_msg.velocity_of_body_in_vision.angular.x = robot_state.kinematic_state.velocity_of_body_in_vision.angular.x ks_msg.velocity_of_body_in_vision.angular.y = robot_state.kinematic_state.velocity_of_body_in_vision.angular.y ks_msg.velocity_of_body_in_vision.angular.z = robot_state.kinematic_state.velocity_of_body_in_vision.angular.z ''' velocity_of_body_in_odom ''' ks_msg.velocity_of_body_in_odom.linear.x = robot_state.kinematic_state.velocity_of_body_in_odom.linear.x ks_msg.velocity_of_body_in_odom.linear.y = robot_state.kinematic_state.velocity_of_body_in_odom.linear.y ks_msg.velocity_of_body_in_odom.linear.z = robot_state.kinematic_state.velocity_of_body_in_odom.linear.z ks_msg.velocity_of_body_in_odom.angular.x = robot_state.kinematic_state.velocity_of_body_in_odom.angular.x ks_msg.velocity_of_body_in_odom.angular.y = robot_state.kinematic_state.velocity_of_body_in_odom.angular.y ks_msg.velocity_of_body_in_odom.angular.z = robot_state.kinematic_state.velocity_of_body_in_odom.angular.z ### BehaviorFaultState conversion '''faults is repeated''' for fault in robot_state.behavior_fault_state.faults: behaviour_fault_state_msg = spot_ros_msgs.msg.BehaviorFaultState() behaviour_fault_state_msg.header.frame_id = fault.behavior_fault_id #[uint32] behaviour_fault_state_msg.header.stamp.secs = fault.onset_timestamp.seconds #[google.protobuf.Timestamp] behaviour_fault_state_msg.header.stamp.nsecs = fault.onset_timestamp.nanos #[google.protobuf.Timestamp] behaviour_fault_state_msg.cause = fault.cause #[enum] behaviour_fault_state_msg.status = fault.status #[enum] rs_msg.behavior_fault_states.append(behaviour_fault_state_msg) ### FootState conversion [repeated] for foot_state in robot_state.foot_state: foot_state_msg = spot_ros_msgs.msg.FootState() foot_state_msg.foot_position_rt_body.x = foot_state.foot_position_rt_body.x #[double] foot_state_msg.foot_position_rt_body.y = foot_state.foot_position_rt_body.y #[double] foot_state_msg.foot_position_rt_body.z = foot_state.foot_position_rt_body.z #[double] foot_state_msg.contact = foot_state.contact #[enum] rs_msg.foot_states.append(foot_state_msg) return ks_msg, rs_msg #kinematic state message, robot state message
def main(argv): parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument('--protocol', default='tcp', type=str, choices=['tcp', 'udp'], help='IP Protocel to use, either tcp or udp.') parser.add_argument('--server-port', default=5201, type=int, help='Port number of iperf3 server') parser.add_argument('--server-hostname', default='127.0.0.1', type=str, help='IP address of ieprf3 server') options = parser.parse_args(argv) sdk = bosdyn.client.create_standard_sdk('CommsTestingClient', [MissionClient]) robot = sdk.create_robot(options.hostname) #ROBOT_IP robot.authenticate(options.username, options.password) robot.sync_with_directory() _robot_state_client = robot.ensure_client( RobotStateClient.default_service_name) _mission_client = robot.ensure_client(MissionClient.default_service_name) _robot_state_task = AsyncRobotState(_robot_state_client) _mission_state_task = AsyncMissionState(_mission_client) _task_list = [_robot_state_task, _mission_state_task] _async_tasks = AsyncTasks(_task_list) print('Connected.') update_thread = threading.Thread(target=_update_thread, args=[_async_tasks]) update_thread.daemon = True update_thread.start() # Wait for the first responses. while any(task.proto is None for task in _task_list): time.sleep(0.1) # list to hold all the data data_list = [] curr_fname = '' try: while True: if _mission_state_task.proto.status != mission_proto.State.STATUS_RUNNING: # Write out data if it exists if len(data_list) > 0: print( f'Finished a mission, data can be found in {curr_fname}' ) data_list.clear() print(_mission_state_task.proto) time.sleep(1) else: if _robot_state_task.proto: # This script currently uses odometry positioning, which is based on the robot's # position at boot time. Runs across boots will not be easily comparable odom_tform_body = get_odom_tform_body( _robot_state_task.proto.kinematic_state. transforms_snapshot).to_proto() helper_se3 = SE3Pose.from_obj(odom_tform_body) #check latency ping_ret = check_ping(options.server_hostname) ping = -1 if ping_ret is None else ping_ret['avg'] # Run iperf3 client client = iperf3.Client() client.duration = 1 client.server_hostname = options.server_hostname client.protocol = options.protocol client.port = options.server_port result = client.run() # update list of data points if result.error: print(result.error) else: data_entry = { 'loc_x': helper_se3.x, 'loc_y': helper_se3.y, 'loc_z': helper_se3.z, 'time': datetime.datetime.now(), 'latency(ms)': ping } if options.protocol == 'udp': data_entry.update({ 'send throughput(Mbps)': result.Mbps, 'recv throughput(Mbps)': -1, 'jitter(ms)': result.jitter_ms, 'lost(%)': result.lost_percent, 'retransmits': -1 }) elif options.protocol == 'tcp': data_entry.update({ 'send throughput(Mbps)': result.sent_Mbps, 'recv throughput(Mbps)': result.received_Mbps, 'jitter(ms)': -1, 'lost(%)': -1, 'retransmits': result.retransmits }) data_list.append(data_entry) print(data_list[-1]) # Create csv with header if it doesn't exist, then write latest row keys = data_list[0].keys() if curr_fname == '': curr_fname = create_csv_filename(options.protocol) with open(curr_fname, 'w') as output_file: header_writer = csv.DictWriter( output_file, keys) header_writer.writeheader() with open(curr_fname, 'a') as output_file: dict_writer = csv.DictWriter(output_file, keys) dict_writer.writerow(data_list[-1]) del client except KeyboardInterrupt: print("Caught KeyboardInterrupt, exiting") return True
def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name='odom', precise_position=False): """Send a trajectory motion command to the robot. Args: goal_x: Position X coordinate in meters goal_y: Position Y coordinate in meters goal_heading: Pose heading in radians cmd_duration: Time-to-live for the command in seconds. frame_name: frame_name to be used to calc the target position. 'odom' or 'vision' precise_position: if set to false, the status STATUS_NEAR_GOAL and STATUS_AT_GOAL will be equivalent. If true, the robot must complete its final positioning before it will be considered to have successfully reached the goal. """ self._at_goal = False self._near_goal = False self._last_trajectory_command_precise = precise_position self._logger.info("got command duration of {}".format(cmd_duration)) end_time = time.time() + cmd_duration if frame_name == 'vision': vision_tform_body = frame_helpers.get_vision_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) vision_tform_goal = vision_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.synchro_se2_trajectory_point_command( goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(), frame_name=frame_helpers.VISION_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) elif frame_name == 'odom': odom_tform_body = frame_helpers.get_odom_tform_body( self._robot_state_client.get_robot_state( ).kinematic_state.transforms_snapshot) body_tform_goal = math_helpers.SE3Pose( x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading)) odom_tform_goal = odom_tform_body * body_tform_goal response = self._robot_command( RobotCommandBuilder.synchro_se2_trajectory_point_command( goal_x=odom_tform_goal.x, goal_y=odom_tform_goal.y, goal_heading=odom_tform_goal.rot.to_yaw(), frame_name=frame_helpers.ODOM_FRAME_NAME, params=self._mobility_params), end_time_secs=end_time) else: raise ValueError('frame_name must be \'vision\' or \'odom\'') if response[0]: self._last_trajectory_command = response[2] return response[0], response[1]