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]
