img_ = rospy.wait_for_message(topic, Image)
img_np_ = ros_numpy.numpify(img_)
PILImage.fromarray(img_np_).save(fp)
i = START_I
while i < int(N_TD):
if init() and scenario_ok():
# save images
for _ in range(RANDS_PER_POSE):
i_str_ = str(i)
i += 1
# randomize object pose(s) and save a 'canonical' image
rand_phys_svc_(EmptyRequest())
rospy.sleep(MJ_PAUSE)
save_camera_image(canonical_img_dir + i_str_ + '.jpg',
'/mujoco/ros_cam/rgb')
# randomize texture(s) and save a 'randomized' image
rand_tex_svc_(EmptyRequest())
save_camera_image(randomized_img_dir + i_str_ + '.jpg',
'/mujoco/ros_cam/rgb')
# de-randomize poses/textures
derand_phys_svc_(EmptyRequest())
rospy.sleep(MJ_PAUSE)
derand_tex_svc_(EmptyRequest())
rospy.sleep(2.)
relaxed_pose()
# Start the look alive behavior (during the turn in place motion).
act_goal = ActGoal()
act_goal.action = 'robot.body.neck.head.ActAlive(5, 2, 5, 2, 2, 10, 2),'
rospy.loginfo("Sending act goal (act alive)...")
act_client.send_goal(act_goal)
act_client.wait_for_result()
# Clear costmaps.
clear_costmaps(EmptyRequest())
###########################################################################
# Waypoints definition.
###########################################################################
# # Turn 180 degrees.
# move_goal.target_pose.header.stamp = rospy.Time.now()
# move_goal.target_pose.pose.position.x = 1.6
# move_goal.target_pose.pose.position.y = -2.0
# move_goal.target_pose.pose.position.z = 0.0
# move_goal.target_pose.pose.orientation = Quaternion(*tr.quaternion_from_euler(0.0, 0.0, -3.14*3/2))
# rospy.loginfo("Sending waypoint 5...")
def load_and_validate_calibration(self):
"""Load the extended calibration file.
Raises:
* IOError if calibration file can't be found.
* ValueError if calibration file is corrupt.
"""
try:
data = numpy.loadtxt(self.RAW_DATA_PATH, delimiter=',')
mag_x = data[:, 0]
mag_y = data[:, 1]
mag_z = data[:, 2]
acc_x = data[:, 3]
acc_y = data[:, 4]
acc_z = data[:, 5]
self.cal['mag_offsets'], self.cal['mag_transform'] = \
self.ellipsoid_fit(mag_x, mag_y, mag_z)
self.cal['acc_offsets'], self.cal['acc_transform'] = \
self.ellipsoid_fit(acc_x, acc_y, acc_z)
self.cal['misalignment'] = [[1., 0, 0], [0, 1., 0], [0, 0, 1.]]
self.cal['gyro_bias'] = [[0], [0], [0]]
self.cal['gyro_scale'] = [[1., 0, 0], [0, 1., 0], [0, 0, 1.]]
rospy.loginfo(self.rover + ': IMU raw data file loaded from ' +
self.RAW_DATA_PATH)
except IOError:
self._diag_fatal(
('FATAL ERROR. Extended calibration file {} ' +
'not found. This rover must be replaced.').format(
self.RAW_DATA_PATH))
rospy.set_param('~imu_is_failed', True)
# Wait in hope the message gets published and received by any
# subscribers before the node exits.
rospy.sleep(3.0)
raise
except ValueError as e:
self._diag_fatal(
('FATAL ERROR. Error reading extended calibration ' +
'file {}. The file is corrupt: {}. This rover must ' +
'be replaced.').format(self.RAW_DATA_PATH, e.message))
rospy.set_param('~imu_is_failed', True)
# Wait in hope the message gets published and received by any
# subscribers before the node exits.
rospy.sleep(3.0)
raise
# Calibration matrices are stored as lists and converted to numpy
# arrays when needed.
self.acc_offsets = self.cal['acc_offsets']
self.acc_transform = self.cal['acc_transform']
self.mag_offsets = self.cal['mag_offsets']
self.mag_transform = self.cal['mag_transform']
self.misalignment = self.cal['misalignment']
self.gyro_bias = self.cal['gyro_bias']
self.gyro_scale = self.cal['gyro_scale']
# Check variance in errors
mag_var_err = self.error(mag_x, mag_y, mag_z, self.mag_offsets,
self.mag_transform)
acc_var_err = self.error(acc_x, acc_y, acc_z, self.acc_offsets,
self.acc_transform)
self._diag_info(('IMU calibration validation: Magnetometer ' +
'v[Err]: {:7.6f}').format(mag_var_err))
self._diag_info(('IMU calibration validation: Accelerometer ' +
'v[Err]: {:7.6f}').format(acc_var_err))
if (math.isnan(mag_var_err)
or abs(mag_var_err) >= IMU.MAG_VAR_TOLERANCE):
self._diag_warn(
("The magnetometer fit is too poor to use. The data's " +
"variance ({:7.6f}) exceeds the threshold of {}. {}").format(
mag_var_err, IMU.MAG_VAR_TOLERANCE, IMU.REPLACEMENT_MSG))
self.needs_calibration = True
self._set_mode(IMU.MODE_2D)
if (math.isnan(acc_var_err)
or abs(acc_var_err) >= IMU.ACC_VAR_TOLERANCE):
self._diag_warn(
("The accelerometer fit is too poor to use. The data's " +
"variance ({:7.6f}) exceeds the threshold of {}. {}").format(
acc_var_err, IMU.ACC_VAR_TOLERANCE, IMU.REPLACEMENT_MSG))
self.needs_calibration = True
self._set_mode(IMU.MODE_2D)
# Check roll and pitch
self.current_state = IMU.STATE_VALIDATE
try:
rospy.wait_for_message('imu/raw', SwarmieIMU, timeout=5)
except rospy.ROSException:
# hopefully this doesn't happen
pass
# wait for 2 seconds for messages to come in and populate
# self.rolls and self.pitches
rospy.sleep(2)
avg_roll = numpy.average(self.rolls) * 180 / math.pi
avg_pitch = numpy.average(self.pitches) * 180 / math.pi
self._diag_info(('IMU calibration validation: ' +
'Average roll: {:6.3f} deg').format(avg_roll))
self._diag_info(('IMU calibration validation: ' +
'Average pitch: {:6.3f} deg').format(avg_pitch))
if abs(avg_roll) > IMU.ROLL_PITCH_TOLERANCE:
self._diag_warn(
('The avg roll exceeds the tolerance threshold of ' +
'{:.1f} deg. {}').format(IMU.ROLL_PITCH_TOLERANCE,
IMU.REPLACEMENT_MSG))
self.needs_calibration = True
self._set_mode(IMU.MODE_2D)
if abs(avg_pitch) > IMU.ROLL_PITCH_TOLERANCE:
self._diag_warn(
('The avg pitch exceeds the tolerance threshold of ' +
'{:.1f} deg. {}').format(IMU.ROLL_PITCH_TOLERANCE,
IMU.REPLACEMENT_MSG))
self.needs_calibration = True
self._set_mode(IMU.MODE_2D)
self.finished_validating = True
self.store_calibration(EmptyRequest())
self.current_state = IMU.STATE_NORMAL
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyRequest
import sys
rospy.init_node('service_client')
rospy.wait_for_service('/global_localization')
disperse_particles_service = rospy.ServiceProxy('/global_localization', Empty)
msg = EmptyRequest()
result = disperse_particles_service(msg)
print result
def reload_params_srv(self, req):
""" Callback of reload params service """
rospy.loginfo('%s: received reload params service', self.name)
self.get_config()
self.reset_timeout_srv(EmptyRequest())
return EmptyResponse()
def enable_teleoperation(self, req):
print "ENABLE TELEOPERATION"
self.teleoperation_enabled = True
self.disable_keep_position(EmptyRequest())
return EmptyResponse()
#!/usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyRequest
if __name__ == '__main__':
rospy.init_node('start_camera_service', anonymous=True)
rospy.wait_for_service('/morpheus_bot/raspicam_node/start_capture')
start_cam = rospy.ServiceProxy('/morpheus_bot/raspicam_node/start_capture',
Empty)
request_e = EmptyRequest()
start_cam(request_e)
rospy.loginfo("Started Camera Service")
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyRequest
rospy.init_node('service_client')
rospy.wait_for_service('move_bb8_in_circle')
serv = rospy.ServiceProxy('move_bb8_in_circle', Empty)
req = EmptyRequest()
res = serv(req)
def __init__(self, racecar_names):
''' Constructor for the Deep Racer object, will load track and waypoints
'''
# Wait for required services to be available
rospy.wait_for_service(SET_MODEL_STATE)
rospy.wait_for_service(GazeboServiceName.PAUSE_PHYSICS.value)
rospy.wait_for_service(GazeboServiceName.GET_MODEL_PROPERTIES.value)
rospy.wait_for_service(GazeboServiceName.GET_VISUAL_NAMES.value)
rospy.wait_for_service(GazeboServiceName.GET_VISUALS.value)
rospy.wait_for_service(GazeboServiceName.SET_VISUAL_COLORS.value)
rospy.wait_for_service(GazeboServiceName.SET_VISUAL_TRANSPARENCIES.value)
rospy.wait_for_service(GazeboServiceName.SET_VISUAL_VISIBLES.value)
self.racer_num = len(racecar_names)
for racecar_name in racecar_names:
wait_for_model(model_name=racecar_name, relative_entity_name='')
self.car_colors = force_list(rospy.get_param(const.YamlKey.CAR_COLOR.value,
[const.CarColorType.BLACK.value] * len(racecar_names)))
self.shell_types = force_list(rospy.get_param(const.YamlKey.BODY_SHELL_TYPE.value,
[const.BodyShellType.DEFAULT.value] * len(racecar_names)))
# Gazebo service that allows us to position the car
self.model_state_client = ServiceProxyWrapper(SET_MODEL_STATE, SetModelState)
self.get_model_prop = ServiceProxyWrapper(GazeboServiceName.GET_MODEL_PROPERTIES.value,
GetModelProperties)
self.get_visual_names = ServiceProxyWrapper(GazeboServiceName.GET_VISUAL_NAMES.value,
GetVisualNames)
self.get_visuals = ServiceProxyWrapper(GazeboServiceName.GET_VISUALS.value, GetVisuals)
self.set_visual_colors = ServiceProxyWrapper(GazeboServiceName.SET_VISUAL_COLORS.value,
SetVisualColors)
self.set_visual_transparencies = ServiceProxyWrapper(GazeboServiceName.SET_VISUAL_TRANSPARENCIES.value,
SetVisualTransparencies)
self.set_visual_visibles = ServiceProxyWrapper(GazeboServiceName.SET_VISUAL_VISIBLES.value,
SetVisualVisibles)
# Place the car at the starting point facing the forward direction
# Instantiate cameras
main_cameras, sub_camera = configure_camera(namespaces=racecar_names)
[camera.detach() for camera in main_cameras.values()]
sub_camera.detach()
# Get the root directory of the ros package, this will contain the models
deepracer_path = rospkg.RosPack().get_path("deepracer_simulation_environment")
# Grab the track data
self.track_data = TrackData.get_instance()
# Set all racers start position in track data
self.start_positions = get_start_positions(self.racer_num)
car_poses = []
for racecar_idx, racecar_name in enumerate(racecar_names):
car_model_state = self.get_initial_position(racecar_name,
racecar_idx)
car_poses.append(car_model_state.pose)
self.update_model_visual(racecar_name,
self.shell_types[racecar_idx],
self.car_colors[racecar_idx])
# Let KVS collect a few frames before pausing the physics, so the car
# will appear on the track
time.sleep(1)
pause_physics = ServiceProxyWrapper('/gazebo/pause_physics', Empty)
logger.info("Pausing physics after initializing the cars")
pause_physics(EmptyRequest())
for racecar_name, car_pose in zip(racecar_names, car_poses):
main_cameras[racecar_name].spawn_model(car_pose,
os.path.join(deepracer_path, "models",
"camera", "model.sdf"))
logger.info("Spawning sub camera model")
# Spawn the top camera model
sub_camera.spawn_model(None, os.path.join(deepracer_path, "models",
"top_camera", "model.sdf"))
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyRequest
rospy.init_node('turtle_circle_service_client')
rospy.wait_for_service('/move_turtle_in_circle')
my_service_client = rospy.ServiceProxy('/move_turtle_in_circle', Empty)
request_object = EmptyRequest()
result = my_service_client(request_object)
print result
def dataset_generator(models_of_interest_list,
models_to_be_list,
full_path_to_dataset_gen='./dataset_gen/',
number_of_dataset_elements=10,
frequency=1,
show_images=False,
env_change_settle_time=2.0,
move_fetch_arm=False,
move_time=2.0,
index_img=0):
# We first wait for the service for RandomEnvironment change to be ready
rospy.loginfo("Waiting for service /dynamic_world_service to be ready...")
rospy.wait_for_service('/dynamic_world_service')
rospy.loginfo("Service /dynamic_world_service READY")
dynamic_world_service_call = rospy.ServiceProxy('/dynamic_world_service',
Empty)
change_env_request = EmptyRequest()
dynamic_world_service_call(change_env_request)
# Init the FetchClient to move the robot arm
move_fetch_client_object = MoveFetchClient()
x_range = [0.35, 0.35]
y_range = [0.1, 0.5]
z_range = [0.64 + 0.3, 0.64 + 0.3]
orientation_XYZW = [-0.707, 0.000, 0.707, 0.001]
#Init Pose
move_fetch_client_object.go_to_safe_arm_pose()
# Set optimum torso height
height = 0.2
move_fetch_client_object.move_torso(height)
# Period show the images in miliseconds
period = int((1 / frequency) * 1000)
# We first check if the path given exists, if not its generated the directory
if os.path.exists(full_path_to_dataset_gen):
shutil.rmtree(full_path_to_dataset_gen)
os.makedirs(full_path_to_dataset_gen)
rospy.logfatal("Created folder=" + str(full_path_to_dataset_gen))
# Init camera RGB object
camera_topic = "/dynamic_objects/camera/raw_image"
camera_info_topic = "/dynamic_objects/camera/info_image"
camera_obj = RGBCamera(camera_topic, camera_info_topic)
# This are the models that we will generate information about.
gz_model_obj = GazeboModel(models_to_be_list)
# XML generator :
xml_generator_obj = XMLGenerator(path_out=full_path_to_dataset_gen)
# index to name the pictures :
now = datetime.datetime.now()
str_date = now.strftime("%Y_%m_%d_%H_%M_")
# create the images folder :
path_img_dir = os.path.join(full_path_to_dataset_gen, 'images/')
if not os.path.exists(path_img_dir):
os.makedirs(path_img_dir)
# We generate as many dataset elements as it was indicated.
for i in range(number_of_dataset_elements):
# We Randomise Environment
dynamic_world_service_call(change_env_request)
rospy.logwarn("Waiting for new env to settle...")
rospy.sleep(env_change_settle_time)
rospy.logwarn("Waiting for new env to settle")
# We move Fetch Arm to random pose
if move_fetch_arm:
rospy.logwarn("Moving to Random Pose TCP")
move_fetch_client_object.move_tcp_to_random_pose(
x_range, y_range, z_range, orientation_XYZW)
rospy.sleep(move_time)
rospy.logwarn("Moving to Random Pose TCP DONE")
else:
move_time = 0.0
estimated_time_of_completion = ((number_of_dataset_elements - i) *
(env_change_settle_time +
(period / 1000.0) + move_time)) / 60.0
rospy.logwarn("Time Estimated of completion [min]==>" +
str(estimated_time_of_completion))
# We take picture
rospy.logerr("Takeing PICTURE")
#image = camera_obj.getImage()
image = camera_obj.get_latest_image()
rospy.logerr("Takeing PICTURE...DONE")
#cam_info = camera_obj.get_camera_info()
cam_info = camera_obj.get_camera_info()
if image is None:
rospy.logwarn("Image value was none")
image = numpy.zeros((cam_info.height, cam_info.width))
filename = str_date + str(index_img)
# We retrieve objects position
pose_models_oi_dict = {}
for model_name in models_of_interest_list:
rospy.logdebug("model_name==>" + str(model_name))
pose_now = gz_model_obj.get_model_pose(model_name)
pose_models_oi_dict[model_name] = pose_now
rospy.logdebug("ObjectsOfInterest poses==>" + str(pose_models_oi_dict))
#raw_input("Press After Validate Position in simulation....")
# We create the XML list tags for each Model of Interest captured before
listobj = []
for model_name, model_3dpose in pose_models_oi_dict.items():
x_com = model_3dpose.position.x
y_com = model_3dpose.position.y
z_com = model_3dpose.position.z
quat_x = model_3dpose.orientation.x
quat_y = model_3dpose.orientation.y
quat_z = model_3dpose.orientation.z
quat_w = model_3dpose.orientation.w
pose3d = [x_com, y_com, z_com, quat_x, quat_y, quat_z, quat_w]
listobj.append(XMLObjectTags(name=model_name, pose3d=pose3d))
image_format_extension = ".png"
xml_generator_obj.generate(object_tags=listobj,
filename=filename,
extension_file=image_format_extension,
camera_width=cam_info.width,
camera_height=cam_info.height)
index_img += 1
file_name_ext = filename + image_format_extension
path_img_file = os.path.join(path_img_dir, file_name_ext)
cv2.imwrite(path_img_file, image)
if show_images:
cv2.imshow(filename, image)
cv2.waitKey(period)
cv2.destroyAllWindows()
def _unpause_gazebo(self):
assert self._config.ros_config.ros_launch_config.gazebo
self._unpause_client.wait_for_service()
self._unpause_client(EmptyRequest())
def goalCB(self, pose_msg):
# if not self.is_working:
# rospy.logwarn("can not receive tf between odom and map, navi function is disabled!")
rospy.loginfo("receive goal pose, pre-process it")
# cancel current goal
self.cancelPub.publish(GoalID())
time.sleep(0.5)
# clear current costmap
self.clearCM.call(EmptyRequest())
time.sleep(0.5)
# get robot pose in map frame
robot_pose = self.pose_map
# get the planned path before actual remap
plan_req = GetPlanRequest()
plan_req.start.header.frame_id = 'map'
plan_req.start.header.stamp = rospy.Time.now()
plan_req.start.pose = robot_pose.pose
plan_goal = pose_msg
# plan_goal = rospy.wait_for_message('/clicked_point', PoseStamped)
plan_req.goal = plan_goal
plan_req.tolerance = 0.1
try:
plan_resp = self.planner.call(plan_req)
except:
rospy.logwarn("failed to make plan, remap goal")
self.goalPub.publish(pose_msg)
plan_path = plan_resp.plan
plan_path.header.stamp = rospy.Time(0)
plan_path.header.frame_id = '/map'
self.planPub.publish(plan_path)
# plan_path = GetPlanResponse()
path_pose = plan_path.poses
if len(path_pose) == 0:
rospy.logwarn(
"failed to find a possible path, remap it only cause the same result!"
)
self.goalPub.publish(pose_msg)
return
dis_array = np.zeros(len(path_pose))
theta_array = np.zeros(len(path_pose))
for i in range(len(path_pose)):
current_pose = path_pose[i]
dis_array[i] = np.hypot(
current_pose.pose.position.x - robot_pose.pose.position.x,
current_pose.pose.position.y - robot_pose.pose.position.y)
theta_array[i] = math.atan2(
current_pose.pose.position.y - robot_pose.pose.position.y,
current_pose.pose.position.x - robot_pose.pose.position.x)
robot_heading = getYaw(robot_pose.pose.orientation)
rospy.loginfo("robot pose-> x:%s, y:%s, theta :%s" %
(robot_pose.pose.position.x, robot_pose.pose.position.y,
rad2deg(robot_heading)))
idx_cond_pre = np.where(dis_array < 0.5)[0]
if len(idx_cond_pre) < 5:
rospy.logwarn("probably too short to get path, remap it only!")
self.goalPub.publish(pose_msg)
return
idx_cond = idx_cond_pre[-1]
theta_sel = theta_array[2:idx_cond + 1]
theta_average = np.mean(theta_sel)
ang_diff = angdiff(theta_average, robot_heading)
if np.abs(ang_diff) > 0.52:
rospy.logwarn(
"planned path at the back of robot, rotate it with angle diff %s"
% rad2deg(ang_diff))
kp, ki, kd = [3.5, 1.0, 0.5]
previous_error = 0
previous_integral = 0
theta_init = self.imu
rat = rospy.Rate(10)
t0 = rospy.get_time()
ts = rospy.get_time()
vel_msg = Twist()
while 1:
theta_current = self.imu
error = angdiff(theta_init + ang_diff, theta_current)
dt = rospy.get_time() - t0
if dt <= 0.0:
dt = 0.01
if math.fabs(error) < 0.035:
vel_msg.angular.z = 0.0
self.velPub.publish(vel_msg)
break
elif (rospy.get_time() - ts) > 15:
vel_msg.angular.z = 0.0
self.velPub.publish(vel_msg)
break
wz, error, integral = PIDcontroller(kp, ki, kd, dt, 'PID',
previous_error, error,
previous_integral)
wz = VelLim(wz, 0.05, 0.8)
previous_error = error
previous_integral = integral
vel_msg.angular.z = wz
self.velPub.publish(vel_msg)
t0 = rospy.get_time()
rat.sleep()
else:
rospy.logwarn(
"planned path right in front of robot, no need to rotate")
self.goalPub.publish(pose_msg)
rospy.loginfo("pre-process finished! republish it to its actual topic")
def evaluation_worker(graph_manager, number_of_trials, task_parameters,
s3_writers, is_continuous):
""" Evaluation worker function
Arguments:
graph_manager {[MultiAgentGraphManager]} -- [Graph manager of multiagent graph manager]
number_of_trials {[int]} -- [Number of trails you want to run the evaluation]
task_parameters {[TaskParameters]} -- [Information of the checkpoint, gpu/cpu, framework etc of rlcoach]
s3_writers {[S3Writer]} -- [Information to upload to the S3 bucket all the simtrace and mp4]
is_continuous {bool} -- [The termination condition for the car]
"""
checkpoint_dirs = list()
agent_names = list()
subscribe_to_save_mp4_topic, unsubscribe_from_save_mp4_topic = list(
), list()
subscribe_to_save_mp4, unsubscribe_from_save_mp4 = list(), list()
for agent_param in graph_manager.agents_params:
_checkpoint_dir = task_parameters.checkpoint_restore_path if len(graph_manager.agents_params) == 1\
else os.path.join(task_parameters.checkpoint_restore_path, agent_param.name)
agent_names.append(agent_param.name)
checkpoint_dirs.append(_checkpoint_dir)
racecar_name = 'racecar' if len(agent_param.name.split("_")) == 1\
else "racecar_{}".format(agent_param.name.split("_")[1])
subscribe_to_save_mp4_topic.append(
"/{}/save_mp4/subscribe_to_save_mp4".format(racecar_name))
unsubscribe_from_save_mp4_topic.append(
"/{}/save_mp4/unsubscribe_from_save_mp4".format(racecar_name))
wait_for_checkpoints(checkpoint_dirs, graph_manager.data_store)
modify_checkpoint_variables(checkpoint_dirs, agent_names)
# Make the clients that will allow us to pause and unpause the physics
rospy.wait_for_service('/gazebo/pause_physics')
rospy.wait_for_service('/gazebo/unpause_physics')
pause_physics = ServiceProxyWrapper('/gazebo/pause_physics', Empty)
unpause_physics = ServiceProxyWrapper('/gazebo/unpause_physics', Empty)
for mp4_sub, mp4_unsub in zip(subscribe_to_save_mp4_topic,
unsubscribe_from_save_mp4_topic):
rospy.wait_for_service(mp4_sub)
rospy.wait_for_service(mp4_unsub)
for mp4_sub, mp4_unsub in zip(subscribe_to_save_mp4_topic,
unsubscribe_from_save_mp4_topic):
subscribe_to_save_mp4.append(ServiceProxyWrapper(mp4_sub, Empty))
unsubscribe_from_save_mp4.append(ServiceProxyWrapper(mp4_unsub, Empty))
graph_manager.create_graph(task_parameters=task_parameters,
stop_physics=pause_physics,
start_physics=unpause_physics,
empty_service_call=EmptyRequest)
logger.info(
"Graph manager successfully created the graph: Unpausing physics")
unpause_physics(EmptyRequest())
graph_manager.reset_internal_state(True)
is_save_mp4_enabled = rospy.get_param('MP4_S3_BUCKET', None)
if is_save_mp4_enabled:
for subscribe_mp4 in subscribe_to_save_mp4:
subscribe_mp4(EmptyRequest())
if is_continuous:
graph_manager.evaluate(EnvironmentSteps(1))
else:
for _ in range(number_of_trials):
graph_manager.evaluate(EnvironmentSteps(1))
if is_save_mp4_enabled:
for unsubscribe_mp4 in unsubscribe_from_save_mp4:
unsubscribe_mp4(EmptyRequest())
for s3_writer in s3_writers:
s3_writer.upload_to_s3()
time.sleep(1)
pause_physics(EmptyRequest())
# Close the down the job
utils.cancel_simulation_job(
os.environ.get('AWS_ROBOMAKER_SIMULATION_JOB_ARN'),
rospy.get_param('AWS_REGION'))
def test_takeoff_double_drone_sim(self):
config = {
"output_path": self.output_dir,
"random_seed": 123,
"robot_name": "double_drone_sim",
"world_name": "empty",
"gazebo": True,
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=[
TopicConfig(topic_name=rospy.get_param(
'/robot/tracking_position_sensor/topic'),
msg_type=rospy.get_param(
'/robot/tracking_position_sensor/type')),
TopicConfig(topic_name=rospy.get_param(
'/robot/fleeing_position_sensor/topic'),
msg_type=rospy.get_param(
'/robot/fleeing_position_sensor/type'))
],
publish_topics=[
TopicConfig(topic_name=rospy.get_param(
'/robot/tracking_command_topic'),
msg_type='Twist'),
TopicConfig(
topic_name=rospy.get_param('/robot/fleeing_command_topic'),
msg_type='Twist')
])
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true(
f'\'{rospy.get_param("/robot/tracking_position_sensor/topic")}\' '
f'in kwargs["ros_topic"].topic_values.keys()',
True,
5,
0.1,
ros_topic=self.ros_topic)
rospy.wait_for_service('/tracking/enable_motors')
enable_motors_service_tracking = rospy.ServiceProxy(
'/tracking/enable_motors', EnableMotors)
rospy.wait_for_service('/fleeing/enable_motors')
enable_motors_service_fleeing = rospy.ServiceProxy(
'/fleeing/enable_motors', EnableMotors)
# set gazebo model state
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
def set_model_state(name: str):
model_state = ModelState()
model_state.pose = Pose()
model_state.model_name = name
model_state.pose.position.x = 0
if 'fleeing' in name:
model_state.pose.position.x += 3. # fixed distance
model_state.pose.position.y = 0
model_state.pose.position.z = 0
yaw = 0 if 'fleeing' not in name else 3.14
model_state.pose.orientation.x, model_state.pose.orientation.y, model_state.pose.orientation.z, \
model_state.pose.orientation.w = quaternion_from_euler((0, 0, yaw))
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
self._pause_client.wait_for_service()
self._pause_client.call()
for _ in range(3):
for model_name in rospy.get_param('/robot/model_name'):
set_model_state(model_name)
self._unpause_client.wait_for_service()
self._unpause_client.call()
# start motors
enable_motors_service_tracking.call(True)
time.sleep(3)
enable_motors_service_fleeing.call(True)
# fly up till reference height
reference = 1.
errors = [1]
while np.mean(errors) > 0.1:
for agent in ['tracking', 'fleeing']:
position = self.ros_topic.topic_values[rospy.get_param(
f'/robot/{agent}_position_sensor/topic')].pose.position
print(f'{agent}: {position.z}')
errors.append(np.abs(reference - position.z))
twist = Twist()
twist.linear.z = +0.5 if position.z < reference else -0.5
self.ros_topic.publishers[rospy.get_param(
f'/robot/{agent}_command_topic')].publish(twist)
while len(errors) > 10:
errors.pop(0)
time.sleep(0.1)
final_error = abs(self.ros_topic.topic_values[rospy.get_param(
'/robot/tracking_position_sensor/topic')].pose.position.z -
reference)
self.assertLess(final_error, 0.2)
final_error = abs(self.ros_topic.topic_values[rospy.get_param(
'/robot/fleeing_position_sensor/topic')].pose.position.z -
reference)
self.assertLess(final_error, 0.2)
self._pause_client.wait_for_service()
self._pause_client(EmptyRequest())
# enable_motors_service_tracking.call(False)
# time.sleep(3)
# enable_motors_service_fleeing.call(False)
ros_process.terminate()
#! /usr/bin/env python
import rospy
# Import the service message used by the service /trajectory_by_name
from std_srvs.srv import Empty, EmptyRequest
from geometry_msgs.msg import Twist
import sys
# Initialise a ROS node with the name service_client
rospy.init_node('service_client')
# Wait for the service client //execute_trajectory to be running
rospy.wait_for_service('/move_bb8_in_circle')
# Create the connection to the service
move_bb8_in_cicle_service = rospy.ServiceProxy('/move_bb8_in_circle', Empty)
# Create an object of type TrajByNameRequest
move_bb8_object = EmptyRequest()
# Send through the connection the name of the trajectory to be executed by the robot
result = move_bb8_in_cicle_service(move_bb8_object)
# Print the result given by the service called
print(result)
def test_takeoff_drone_sim(self):
config = {
"output_path": self.output_dir,
"random_seed": 123,
"robot_name": "drone_sim",
"world_name": "empty",
"gazebo": True,
}
ros_process = RosWrapper(launch_file='load_ros.launch',
config=config,
visible=False)
self._unpause_client = rospy.ServiceProxy('/gazebo/unpause_physics',
Emptyservice)
self._pause_client = rospy.ServiceProxy('/gazebo/pause_physics',
Emptyservice)
self.ros_topic = TestPublisherSubscriber(
subscribe_topics=[
TopicConfig(
topic_name=rospy.get_param('/robot/position_sensor/topic'),
msg_type=rospy.get_param('/robot/position_sensor/type'))
],
publish_topics=[
TopicConfig(topic_name=rospy.get_param('/robot/command_topic'),
msg_type='Twist')
])
# unpause gazebo to receive messages
self._unpause_client.wait_for_service()
self._unpause_client.call()
safe_wait_till_true(
f'\'{rospy.get_param("/robot/position_sensor/topic")}\' '
f'in kwargs["ros_topic"].topic_values.keys()',
True,
5,
0.1,
ros_topic=self.ros_topic)
rospy.wait_for_service('/enable_motors')
enable_motors_service = rospy.ServiceProxy('/enable_motors',
EnableMotors)
enable_motors_service.call(False)
self._pause_client.wait_for_service()
self._pause_client.call()
self._set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
for _ in range(3):
# set gazebo model state
model_state = ModelState()
model_state.model_name = 'quadrotor'
model_state.pose = Pose()
self._set_model_state.wait_for_service()
self._set_model_state(model_state)
self._unpause_client.wait_for_service()
self._unpause_client.call()
enable_motors_service.call(True)
# fly up till reference height
reference = 1.
errors = [1]
while np.mean(errors) > 0.05:
position = self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')].pose.pose.position
print(position.z)
errors.append(np.abs(reference - position.z))
if len(errors) > 5:
errors.pop(0)
twist = Twist()
twist.linear.z = +0.5 if position.z < reference else -0.5
self.ros_topic.publishers[rospy.get_param(
'/robot/command_topic')].publish(twist)
time.sleep(0.1)
final_error = abs(self.ros_topic.topic_values[rospy.get_param(
'/robot/position_sensor/topic')].pose.pose.position.z -
reference)
self.assertLess(final_error, 0.1)
self._pause_client.wait_for_service()
self._pause_client(EmptyRequest())
enable_motors_service.call(False)
ros_process.terminate()
#! /usr/bin/env python
import rospy
from std_srvs.srv import Empty, EmptyRequest
rospy.init_node('particle_initialization_node')
rospy.wait_for_service('/global_localization')
particle_client = rospy.ServiceProxy('/global_localization', Empty)
result = particle_client(EmptyRequest())
print result
def clear_octomap(self):
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0)
start = rospy.Time.now()
allotted = rospy.Duration(60)
remaining = allotted
while remaining.to_sec() > 0:
elapsed = rospy.Time.now() - start
remaining = allotted - elapsed
subprocess.call("clear") # clear the command line
print "Time Left:", max(remaining.to_sec(), 0)
sleep(.1)
# game is over
lynx.stop() # must stop before pausing physics
pause_physics_client(EmptyRequest())
subprocess.call("clear")
[names, poses, twists] = lynx.get_object_state()
score(names, poses, verbose=True) # print detailed score report
# kill student code
[kill(i) for i in range(4)]
except KeyboardInterrupt:
print("Aborted round, pausing physics and killing student code...")
pause_physics_client(EmptyRequest())
[kill(i) for i in range(4)]
lynx.stop()
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
rospy.loginfo("Adding new 'part' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
#Add object description in scene
self.scene.add_box("part", object_pose, (self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
table_pose1 = copy.deepcopy(object_pose)
table_pose2 = copy.deepcopy(object_pose)
table_pose3 = copy.deepcopy(object_pose)
#define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_height/2
table_width = 1.5
table_depth = 0.16
table_pose.pose.position.z += -(self.object_height)/2 -table_height / 2
table_height -= 0.02 #remove few milimeters to prevent contact between the object and the table
table_height1 = 0.5
table_width1 = 0.02
table_depth1 = 0.20
table_pose1.pose.position.z += -(self.object_height) / 2 + table_height1 / 2
table_pose1.pose.position.y += 0.2
table_height1 -= 0.008 # remove few milimeters to prevent contact between the object and the table
table_height2 = 0.5
table_width2 = 0.02
table_depth2 = 0.20
table_pose2.pose.position.z += -(self.object_height) / 2 + table_height2 / 2
table_pose2.pose.position.y -= 0.2
table_height2 -= 0.008 # remove few milimeters to prevent contact between the object and the table
table_height3 = 0.04
table_width3 = 1.5
table_depth3 = 0.20
table_pose3.pose.position.z += -(self.object_height) / 2 + table_height3 / 2 + 0.3
table_height3 -= 0.008 # remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
self.scene.add_box("table1", table_pose1, (table_depth1, table_width1, table_height1)) #add by chen
self.scene.add_box("table2", table_pose2, (table_depth2, table_width2, table_height2)) #add by chen
self.scene.add_box("table3", table_pose3, (table_depth3, table_width3, table_height3)) # add by chen
# # We need to wait for the object part to appear
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("table")
self.wait_for_planning_scene_object("table1")
self.wait_for_planning_scene_object("table2")
self.wait_for_planning_scene_object("table3")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
self.pickup_ac
goal = createPickupGoal(
"arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo(
"Pick result: " +
str(moveit_error_dict[result.error_code.val]))
return result.error_code.val
def rollout_worker(graph_manager, num_workers, rollout_idx, task_parameters,
simtrace_video_s3_writers, pause_physics, unpause_physics):
"""
wait for first checkpoint then perform rollouts using the model
"""
if not graph_manager.data_store:
raise AttributeError("None type for data_store object")
is_sageonly = check_is_sageonly()
data_store = graph_manager.data_store
#TODO change agent to specific agent name for multip agent case
checkpoint_dir = os.path.join(task_parameters.checkpoint_restore_path,
"agent")
graph_manager.data_store.wait_for_checkpoints()
graph_manager.data_store.wait_for_trainer_ready()
# wait for the required cancel services to become available
# Do this only for Robomaker job.
# if not is_sageonly:
# rospy.wait_for_service('/robomaker/job/cancel') # Make the clients that will allow us to pause and unpause the physics
rospy.wait_for_service('/racecar/save_mp4/subscribe_to_save_mp4')
rospy.wait_for_service('/racecar/save_mp4/unsubscribe_from_save_mp4')
subscribe_to_save_mp4 = ServiceProxyWrapper(
'/racecar/save_mp4/subscribe_to_save_mp4', Empty)
unsubscribe_from_save_mp4 = ServiceProxyWrapper(
'/racecar/save_mp4/unsubscribe_from_save_mp4', Empty)
graph_manager.create_graph(task_parameters=task_parameters,
stop_physics=pause_physics,
start_physics=unpause_physics,
empty_service_call=EmptyRequest)
chkpt_state_reader = CheckpointStateReader(checkpoint_dir,
checkpoint_state_optional=False)
last_checkpoint = chkpt_state_reader.get_latest().num
# this worker should play a fraction of the total playing steps per rollout
episode_steps_per_rollout = graph_manager.agent_params.algorithm.num_consecutive_playing_steps.num_steps
act_steps = int(episode_steps_per_rollout / num_workers)
if rollout_idx < episode_steps_per_rollout % num_workers:
act_steps += 1
act_steps = EnvironmentEpisodes(act_steps)
configure_environment_randomizer()
for _ in range(
(graph_manager.improve_steps / act_steps.num_steps).num_steps):
# Collect profiler information only IS_PROFILER_ON is true
with utils.Profiler(s3_bucket=PROFILER_S3_BUCKET,
s3_prefix=PROFILER_S3_PREFIX,
output_local_path=ROLLOUT_WORKER_PROFILER_PATH,
enable_profiling=IS_PROFILER_ON):
graph_manager.phase = RunPhase.TRAIN
exit_if_trainer_done(checkpoint_dir, simtrace_video_s3_writers,
rollout_idx)
unpause_physics(EmptyRequest())
graph_manager.reset_internal_state(True)
graph_manager.act(act_steps,
wait_for_full_episodes=graph_manager.
agent_params.algorithm.act_for_full_episodes)
graph_manager.reset_internal_state(True)
time.sleep(1)
pause_physics(EmptyRequest())
graph_manager.phase = RunPhase.UNDEFINED
new_checkpoint = -1
if graph_manager.agent_params.algorithm.distributed_coach_synchronization_type\
== DistributedCoachSynchronizationType.SYNC:
unpause_physics(EmptyRequest())
is_save_mp4_enabled = rospy.get_param(
'MP4_S3_BUCKET', None) and rollout_idx == 0
if is_save_mp4_enabled:
subscribe_to_save_mp4(EmptyRequest())
if rollout_idx == 0:
for _ in range(int(rospy.get_param('MIN_EVAL_TRIALS',
'5'))):
graph_manager.evaluate(EnvironmentSteps(1))
# For sageonly job for better performance only run limited number of evaluations.
# Pausing the physics makes its performance same as RoboMaker + SageMaker
if is_sageonly:
if is_save_mp4_enabled:
unsubscribe_from_save_mp4(EmptyRequest())
# upload simtrace and mp4 into s3 bucket
for s3_writer in simtrace_video_s3_writers:
s3_writer.persist(utils.get_s3_kms_extra_args())
graph_manager.phase = RunPhase.WAITING
pause_physics(EmptyRequest())
while new_checkpoint < last_checkpoint + 1:
exit_if_trainer_done(checkpoint_dir,
simtrace_video_s3_writers,
rollout_idx)
# Continously run the evaluation only for SageMaker + RoboMaker job
if not is_sageonly and rollout_idx == 0:
print(
"Additional evaluation. New Checkpoint: {}, Last Checkpoint: {}"
.format(new_checkpoint, last_checkpoint))
graph_manager.evaluate(EnvironmentSteps(1))
else:
time.sleep(5)
new_checkpoint = data_store.get_coach_checkpoint_number(
'agent')
# Save the mp4 for Robo+Sage jobs
if not is_sageonly:
if is_save_mp4_enabled:
unsubscribe_from_save_mp4(EmptyRequest())
# upload simtrace and mp4 into s3 bucket
for s3_writer in simtrace_video_s3_writers:
s3_writer.persist(utils.get_s3_kms_extra_args())
pause_physics(EmptyRequest())
data_store.load_from_store(
expected_checkpoint_number=last_checkpoint + 1)
graph_manager.restore_checkpoint()
if graph_manager.agent_params.algorithm.distributed_coach_synchronization_type\
== DistributedCoachSynchronizationType.ASYNC:
if new_checkpoint > last_checkpoint:
graph_manager.restore_checkpoint()
last_checkpoint = new_checkpoint
logger.info("Exited main loop. Done.")
def place_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
#self.scene.remove_attached_object("arm_tool_link")
#self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
self.scene.remove_world_object("table1")
self.scene.remove_world_object("table2")
self.scene.remove_world_object("table3")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0) # Removing is fast
#rospy.loginfo("Adding new 'part2' object")
rospy.loginfo("Object pose: %s", object_pose.pose)
# Add object description in scene
#self.scene.add_box("part2", object_pose, (self.object_depth, self.object_width, self.object_height))
rospy.loginfo("Second%s", object_pose.pose)
table_pose = copy.deepcopy(object_pose)
# define a virtual table below the object
table_height = object_pose.pose.position.z - self.object_height / 2
table_width = 1.5
table_depth = 0.6
table_pose.pose.position.z += -(self.object_height) / 2 - table_height / 2
table_height -= 0.02 # remove few milimeters to prevent contact between the object and the table
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
#self.wait_for_planning_scene_object("part2")
self.wait_for_planning_scene_object("table")
possible_placings = self.sg.create_placings_from_object_pose(
object_pose)
# Try only with arm
rospy.loginfo("Trying to place using only arm")
goal = createPlaceGoal(
object_pose, possible_placings, "arm", "part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.loginfo(str(moveit_error_dict[result.error_code.val]))
if str(moveit_error_dict[result.error_code.val]) != "SUCCESS":
rospy.loginfo(
"Trying to place with arm and torso")
# Try with arm and torso
goal = createPlaceGoal(
object_pose, possible_placings, "arm_torso", "part", self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.place_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.place_ac.wait_for_result()
result = self.place_ac.get_result()
rospy.logerr(str(moveit_error_dict[result.error_code.val]))
# print result
rospy.loginfo(
"Result: " +
str(moveit_error_dict[result.error_code.val]))
rospy.loginfo("Removing previous 'part' object")
#self.scene.remove_world_object("part")
return result.error_code.val
def imu_callback(self, imu_raw):
"""Callback for the SwarmieIMU message containing raw accelerometer and
magnetometer data.
Calibrates IMU by fitting an ellipsoid, or calculating the misalignment
matrix if we are in either of those states.
Computes calibrated accelerometer and magnetometer data, transformed
from the IMU's frame into the rover's frame, calculates roll, pitch,
yaw, and publishes a calibrated IMU message.
"""
if self.current_state == IMU.STATE_IDLE:
return
if self.DEBUG:
# Message for raw, calibrated data. Published only when debugging
imu_raw_cal = SwarmieIMU()
imu_raw_cal.header = imu_raw.header
imu_raw_cal.angular_velocity = imu_raw.angular_velocity
# IMU Message
imu_cal = Imu()
imu_cal.header = imu_raw.header
imu_cal.orientation_covariance[8] = 0.00004
imu_cal.angular_velocity_covariance[8] = 0.00001
if self.current_state == IMU.STATE_CAL_GYRO_BIAS:
self.gyro_data[0].append(imu_raw.angular_velocity.x)
self.gyro_data[1].append(imu_raw.angular_velocity.y)
self.gyro_data[2].append(imu_raw.angular_velocity.z)
self.gyro_bias[0][0] = float(numpy.mean(self.gyro_data[0]))
self.gyro_bias[1][0] = float(numpy.mean(self.gyro_data[1]))
self.gyro_bias[2][0] = float(numpy.mean(self.gyro_data[2]))
(gyro_x, gyro_y,
gyro_z) = self.compute_calibrated_data(imu_raw.angular_velocity.x,
imu_raw.angular_velocity.y,
imu_raw.angular_velocity.z,
self.gyro_bias,
self.gyro_scale,
use_misalignment=False)
if self.DEBUG:
imu_raw_cal.angular_velocity.x = gyro_x
imu_raw_cal.angular_velocity.y = gyro_y
imu_raw_cal.angular_velocity.z = gyro_z
# Convert gyroscope digits to millidegrees per second, then to degrees
# per second, and finally to radians per second. axes mismatched as in
# original arduino code to match rover's coord frame
imu_cal.angular_velocity.x = gyro_y * 8.75 / 1000 * (math.pi / 180)
imu_cal.angular_velocity.y = -gyro_x * 8.75 / 1000 * (math.pi / 180)
imu_cal.angular_velocity.z = gyro_z * 8.75 / 1000 * (math.pi / 180)
if self.current_state == IMU.STATE_CAL_GYRO_SCALE:
current_time = rospy.Time.now().to_sec()
if current_time - self.gyro_start_time < 10:
self.gyro_status_msg = (
self.rover + ': Collecting data from first gyro rotation.')
self.gyro_data[0].append(imu_raw.header.stamp.to_sec())
self.gyro_data[1].append(imu_cal.angular_velocity.z)
elif current_time - self.gyro_start_time < 20:
self.gyro_status_msg = (
self.rover +
': Collecting data from second gyro rotation.')
self.gyro_data[2].append(imu_raw.header.stamp.to_sec())
self.gyro_data[3].append(imu_cal.angular_velocity.z)
else:
angle_1 = numpy.trapz(self.gyro_data[1], x=self.gyro_data[0])
angle_2 = numpy.trapz(self.gyro_data[3], x=self.gyro_data[2])
z_scale = (abs(math.pi / angle_1) + abs(math.pi / angle_2)) / 2
self.gyro_scale[2][2] = z_scale
msg = (self.rover +
': Finished collecting gyro rotation data. ' +
'Z-Scale: ' + str(z_scale))
self.info_log.publish(msg)
self.store_calibration(EmptyRequest())
if self.current_state == IMU.STATE_CAL_MAG:
self.mag_data[0].append(imu_raw.magnetometer.x)
self.mag_data[1].append(imu_raw.magnetometer.y)
self.mag_data[2].append(imu_raw.magnetometer.z)
if len(self.mag_data[0]) > IMU.DATA_SIZE_LIMIT:
rospy.logwarn('IMU calibration timeout exceeded. ' +
'Saving current calculated matrix.')
self.store_calibration(EmptyRequest())
# Don't fit until some data has been collected. May still get a
# runtime warning until enough data in all directions has been
# collected.
elif len(self.mag_data[0]) > IMU.MIN_DATA_SIZE:
self.mag_offsets, self.mag_transform = self.ellipse_fit(
numpy.array(self.mag_data[0]),
numpy.array(self.mag_data[1]))
self.publish_diagnostic_msg()
if self.current_mode == IMU.MODE_3D:
(acc_x, acc_y, acc_z) = self.compute_calibrated_data(
imu_raw.accelerometer.x, imu_raw.accelerometer.y,
imu_raw.accelerometer.z, self.acc_offsets, self.acc_transform)
elif self.current_mode == IMU.MODE_2D:
# Convert accelerometer digits to milligravities, then to
# gravities
acc_x = imu_raw.accelerometer.x * 0.061 / 1000
acc_y = imu_raw.accelerometer.y * 0.061 / 1000
acc_z = imu_raw.accelerometer.z * 0.061 / 1000
if self.DEBUG:
imu_raw_cal.accelerometer.x = acc_x
imu_raw_cal.accelerometer.y = acc_y
imu_raw_cal.accelerometer.z = acc_z
# swap x and y to orient measurements in the rover's frame
tmp = -acc_x
acc_x = acc_y
acc_y = tmp
# Scale accelerations back to m/s**2.
imu_cal.linear_acceleration.x = acc_x * 9.81
imu_cal.linear_acceleration.y = acc_y * 9.81
imu_cal.linear_acceleration.z = acc_z * 9.81
(mag_x, mag_y, mag_z) = self.compute_calibrated_data(
imu_raw.magnetometer.x, imu_raw.magnetometer.y,
imu_raw.magnetometer.z, self.mag_offsets, self.mag_transform)
if self.DEBUG:
imu_raw_cal.magnetometer.x = mag_x
imu_raw_cal.magnetometer.y = mag_y
imu_raw_cal.magnetometer.z = mag_z
if self.current_state == IMU.STATE_CAL_MISALIGN:
self.mag_data[0].append(mag_x)
self.mag_data[1].append(mag_y)
self.mag_data[2].append(mag_z)
if len(self.mag_data[0]) > IMU.DATA_SIZE_LIMIT:
rospy.logwarn('Misalignment calibration timeout exceeded. ' +
'Saving current calculated matrix.')
self.store_calibration(EmptyRequest())
# Wait until some data has been collected.
elif len(self.mag_data[0]) > IMU.MIN_DATA_SIZE:
data = numpy.array(self.mag_data)
self.misalignment = self.calc_misalignment(
data.T, self.misalignment)
self.publish_diagnostic_msg()
# swap x and y to orient measurements in the rover's frame Done here so
# the misalignment calibration can use data in the original IMU frame
tmp = -mag_x
mag_x = mag_y
mag_y = tmp
# From: Computing tilt measurement and tilt-compensated e-compass
# www.st.com/resource/en/design_tip/dm00269987.pdf
self.roll = math.atan2(acc_y, acc_z)
Gz2 = (acc_y * math.sin(self.roll) + acc_z * math.cos(self.roll))
# Gimbal-lock. Special case when pitched + or - 90 deg.
# Heading is unreliable here, but plenty of other bad things will be
# happening if the rover is ever in this position.
if abs(Gz2) < 0.01:
if acc_x > 0:
rospy.loginfo('Special compass case: pitch is -90 deg')
self.pitch = -math.pi / 2
else:
rospy.loginfo('Special compass case: pitch is +90 deg')
self.pitch = math.pi / 2
alpha = .01 # Can be set from [0.01 - 0.05]
self.roll = math.atan2(acc_y, acc_z + acc_x * alpha)
else:
self.pitch = math.atan(-acc_x / Gz2)
By2 = mag_z * math.sin(self.roll) - mag_y * math.cos(self.roll)
Bz2 = mag_y * math.sin(self.roll) + mag_z * math.cos(self.roll)
Bx3 = mag_x * math.cos(self.pitch) + Bz2 * math.sin(self.pitch)
self.yaw = math.pi / 2 + math.atan2(By2, Bx3)
imu_cal.orientation = Quaternion(
*tf.transformations.quaternion_from_euler(self.roll, self.pitch,
self.yaw))
if self.current_state == IMU.STATE_VALIDATE:
self.rolls.append(self.roll)
self.pitches.append(self.pitch)
if self.DEBUG:
self.imu_cal_data_pub.publish(imu_raw_cal)
self.imu_pub.publish(imu_cal)
return
def __init__(self):
parser = argparse.ArgumentParser()
parser.add_argument("data_path", type=str, help="path to data")
parser.add_argument("-l",
"--loop",
action='store_true',
help="loop over files")
parser.add_argument("-s",
"--service",
action='store_true',
help="publish files on service call")
parser.add_argument("-f",
"--frequency",
type=float,
default=30.0,
help="replay frequency")
parser.add_argument("-i",
"--index",
type=int,
default=None,
help="start index of image range")
parser.add_argument("-j",
"--end_range",
type=int,
default=None,
help="end index of image range")
parser.add_argument("-p",
"--print_file",
action='store_true',
default=False,
help="print current file name")
parser.add_argument("-c",
"--cutoff",
type=int,
help="cutoff depth value in millimeter")
parser.add_argument("-t",
"--time",
action='store_true',
help="stamp messages with real wall clock time")
parser.add_argument("-bf",
"--base_frame",
type=str,
default="world",
help="base frame")
parser.add_argument("-cf",
"--camera_frame",
type=str,
default="camera_rgb_optical_frame",
help="camera frame")
parser.add_argument("--skip", type=int, help="skip frames")
parser.add_argument(
"--raw",
action='store_true',
help="decode PNG files and publish raw image messages")
self.args = parser.parse_args(args=rospy.myargv()[1:])
if not os.path.isdir(self.args.data_path):
parser.error("directory '" + self.args.data_path +
"' does not exist")
clist = sorted(glob.glob(
os.path.join(self.args.data_path, "colour", "*.png")),
key=lambda x: int(filter(str.isdigit, x)))
dlist = sorted(glob.glob(
os.path.join(self.args.data_path, "depth", "*.png")),
key=lambda x: int(filter(str.isdigit, x)))
if len(clist) == 0 or len(dlist) == 0:
raise UserWarning("no images found")
if len(clist) != len(dlist):
raise UserWarning("number of images mismatch")
index = np.arange(0, len(clist))
js = None
for jf in ["sol_joints.csv", "joints.csv"]:
joint_file_path = os.path.join(self.args.data_path, jf)
if os.path.isfile(joint_file_path):
js = np.genfromtxt(joint_file_path,
dtype=None,
encoding="utf8",
names=True)
break
if js is not None:
self.joint_names = js.dtype.names
else:
raise UserWarning("no joint file found")
if self.args.index is not None:
if self.args.end_range is not None:
clist = clist[self.args.index:self.args.end_range]
dlist = dlist[self.args.index:self.args.end_range]
js = js[self.args.index:self.args.end_range]
index = index[self.args.index:self.args.end_range]
else:
clist = [clist[self.args.index]]
dlist = [dlist[self.args.index]]
js = [js[self.args.index]]
index = [index[self.args.index]]
if self.args.skip:
clist = clist[::self.args.skip]
dlist = dlist[::self.args.skip]
js = js[::self.args.skip]
index = index[::self.args.skip]
with open(os.path.join(self.args.data_path,
"camera_parameters.json")) as f:
cp = json.load(f)
assert (len(clist) == len(dlist))
# list of camera poses per image, order: px py pz, qw qx qy qz
camera_poses = np.loadtxt(os.path.join(self.args.data_path,
"camera_pose.csv"),
skiprows=1,
delimiter=' ')
vicon_path = os.path.join(self.args.data_path,
"vicon_pose_lwr_end_effector.csv")
if os.path.exists(vicon_path):
self.vicon_poses = np.loadtxt(os.path.join(
self.args.data_path, "vicon_pose_lwr_end_effector.csv"),
skiprows=0,
delimiter=' ')
else:
self.vicon_poses = None
rospy.init_node("export_player")
self.pub_clock = rospy.Publisher("/clock", Clock, queue_size=1)
if self.args.raw:
self.pub_colour = rospy.Publisher("/camera/rgb/image_rect_color",
Image,
queue_size=1)
self.pub_depth = rospy.Publisher("/camera/depth/image_rect_raw",
Image,
queue_size=1)
self.pub_colour_compressed = rospy.Publisher(
"/camera/rgb/image_rect_color/compressed",
CompressedImage,
queue_size=1)
self.pub_ci_colour = rospy.Publisher("/camera/rgb/camera_info",
CameraInfo,
latch=True,
queue_size=1)
self.pub_depth_compressed = rospy.Publisher(
"/camera/depth/image_rect_raw/compressed",
CompressedImage,
queue_size=1)
self.pub_depth_compressed_depth = rospy.Publisher(
"/camera/depth/image_rect_raw/compressedDepth",
CompressedImage,
queue_size=1)
self.pub_ci_depth = rospy.Publisher("/camera/depth/camera_info",
CameraInfo,
latch=True,
queue_size=1)
self.pub_joints = rospy.Publisher("/joint_states",
JointState,
latch=True,
queue_size=1)
self.pub_eol = rospy.Publisher("~end_of_log",
Bool,
queue_size=1,
latch=True)
self.pub_id = rospy.Publisher("~id", UInt64, queue_size=1, latch=True)
self.ci = CameraInfo(
width=cp['width'],
height=cp['height'],
K=[cp['fu'], 0, cp['cx'], 0, cp['fv'], cp['cy'], 0, 0, 1],
P=[cp['fu'], 0, cp['cx'], 0, 0, cp['fv'], cp['cy'], 0, 0, 0, 1, 0])
self.last_filename = None
self.new_file = True
self.cvbridge = cv_bridge.CvBridge()
self.broadcaster = tf2_ros.TransformBroadcaster()
if self.args.time:
# timestamps will be ignored and replaced by current time
timestamps = [None] * len(clist)
else:
# load time stamps in nanoseconds
timestamps = np.loadtxt(os.path.join(self.args.data_path,
"time.csv"),
dtype=np.uint).tolist()
self.file_list = zip(index, timestamps, clist, dlist, js, camera_poses)
self.i = 0
self.N = len(self.file_list)
print("samples:", self.N)
if not self.args.loop:
self.pub_eol.publish(data=False)
if self.args.service:
rospy.Service("~reset", Empty, self.reset_iter)
self.reset_iter(EmptyRequest())
rospy.Service("~next", Empty, self.next_set)
rospy.spin()
else:
loop = True
while loop and not rospy.is_shutdown():
for self.i in range(self.N):
tstart = time.time()
ind, t, cpath, dpath, jp, cpose = self.file_list[self.i]
self.send(cpath, dpath, jp, t, ind, cpose)
dur = time.time() - tstart
if rospy.is_shutdown():
break
delay = (1 / float(self.args.frequency) - dur)
delay = max(0, delay)
time.sleep(delay)
loop = self.args.loop
if not self.args.loop:
self.pub_eol.publish(data=True)
#! /usr/bin/env python
import rospkg
import rospy
from std_srvs.srv import Empty, EmptyRequest
rospy.init_node('service_move_bb8_in_square_client')
rospy.wait_for_service('/move_bb8_in_square')
move_bb8_in_square_service_client = rospy.ServiceProxy('/move_bb8_in_square',
Empty)
move_bb8_in_square_request_object = EmptyRequest()
result = move_bb8_in_square_service_client(move_bb8_in_square_request_object)
print result
def wake_up():
su.call_service("/naoqi_driver/motion/wake_up", Empty, EmptyRequest())
def init():
reset_svc_(EmptyRequest())
rospy.sleep(0.1)
return grab_the_nut(jpos_pub_, gripper_pub_)
def rest():
su.call_service("/naoqi_driver/motion/rest", Empty, EmptyRequest())
def evaluation_worker(graph_manager, number_of_trials, task_parameters, s3_writers, is_continuous,
park_positions):
""" Evaluation worker function
Arguments:
graph_manager(MultiAgentGraphManager): Graph manager of multiagent graph manager
number_of_trials(int): Number of trails you want to run the evaluation
task_parameters(TaskParameters): Information of the checkpoint, gpu/cpu,
framework etc of rlcoach
s3_writers(S3Writer): Information to upload to the S3 bucket all the simtrace and mp4
is_continuous(bool): The termination condition for the car
park_positions(list of tuple): list of (x, y) for cars to park at
"""
# Collect profiler information only IS_PROFILER_ON is true
with utils.Profiler(s3_bucket=PROFILER_S3_BUCKET, s3_prefix=PROFILER_S3_PREFIX,
output_local_path=ROLLOUT_WORKER_PROFILER_PATH, enable_profiling=IS_PROFILER_ON):
checkpoint_dirs = list()
agent_names = list()
subscribe_to_save_mp4_topic, unsubscribe_from_save_mp4_topic = list(), list()
subscribe_to_save_mp4, unsubscribe_from_save_mp4 = list(), list()
for agent_param in graph_manager.agents_params:
_checkpoint_dir = task_parameters.checkpoint_restore_path if len(graph_manager.agents_params) == 1 \
else os.path.join(task_parameters.checkpoint_restore_path, agent_param.name)
agent_names.append(agent_param.name)
checkpoint_dirs.append(_checkpoint_dir)
racecar_name = 'racecar' if len(agent_param.name.split("_")) == 1 \
else "racecar_{}".format(agent_param.name.split("_")[1])
subscribe_to_save_mp4_topic.append("/{}/save_mp4/subscribe_to_save_mp4".format(racecar_name))
unsubscribe_from_save_mp4_topic.append("/{}/save_mp4/unsubscribe_from_save_mp4".format(racecar_name))
wait_for_checkpoints(checkpoint_dirs, graph_manager.data_store)
modify_checkpoint_variables(checkpoint_dirs, agent_names)
# Make the clients that will allow us to pause and unpause the physics
rospy.wait_for_service('/gazebo/pause_physics_dr')
rospy.wait_for_service('/gazebo/unpause_physics_dr')
pause_physics = ServiceProxyWrapper('/gazebo/pause_physics_dr', Empty)
unpause_physics = ServiceProxyWrapper('/gazebo/unpause_physics_dr', Empty)
for mp4_sub, mp4_unsub in zip(subscribe_to_save_mp4_topic, unsubscribe_from_save_mp4_topic):
rospy.wait_for_service(mp4_sub)
rospy.wait_for_service(mp4_unsub)
for mp4_sub, mp4_unsub in zip(subscribe_to_save_mp4_topic, unsubscribe_from_save_mp4_topic):
subscribe_to_save_mp4.append(ServiceProxyWrapper(mp4_sub, Empty))
unsubscribe_from_save_mp4.append(ServiceProxyWrapper(mp4_unsub, Empty))
graph_manager.create_graph(task_parameters=task_parameters, stop_physics=pause_physics,
start_physics=unpause_physics, empty_service_call=EmptyRequest)
logger.info("Graph manager successfully created the graph: Unpausing physics")
unpause_physics(EmptyRequest())
is_save_mp4_enabled = rospy.get_param('MP4_S3_BUCKET', None)
if is_save_mp4_enabled:
for subscribe_mp4 in subscribe_to_save_mp4:
subscribe_mp4(EmptyRequest())
configure_environment_randomizer()
track_data = TrackData.get_instance()
# Before each evaluation episode (single lap for non-continuous race and complete race for
# continuous race), a new copy of park_positions needs to be loaded into track_data because
# a park position will be pop from park_positions when a racer car need to be parked.
if is_continuous:
track_data.park_positions = park_positions
graph_manager.evaluate(EnvironmentSteps(1))
else:
for _ in range(number_of_trials):
track_data.park_positions = park_positions
graph_manager.evaluate(EnvironmentSteps(1))
if is_save_mp4_enabled:
for unsubscribe_mp4 in unsubscribe_from_save_mp4:
unsubscribe_mp4(EmptyRequest())
for s3_writer in s3_writers:
s3_writer.upload_to_s3()
time.sleep(1)
pause_physics(EmptyRequest())
# Close the down the job
utils.cancel_simulation_job(os.environ.get('AWS_ROBOMAKER_SIMULATION_JOB_ARN'),
rospy.get_param('AWS_REGION'))
def use_knob(request):
""" Move to standoff, then use knob """
# Unpack request message
which_arm = request.which_arm
name = request.knob
offset = request.goal_offset
standoff = request.standoff
listener = tf.TransformListener()
# Get close to knob
go_to_knob(which_arm, name, standoff + 0.05, offset, 0.20) # align with knob
gripper = Gripper()
# Open gripper & re-calibrate pressure sensors
gripper.open()
calibrate_pressure = rospy.ServiceProxy('calibrate_pressure', Empty_srv)
request = EmptyRequest()
calibrate_pressure(request)
# Engage with knob
response = go_to_knob(which_arm, name, standoff, offset, 0.20) # move to standoff quickly
response = go_to_knob(which_arm, name, -0.10, offset, 0.05) # move towards knob slowly until sense contact
response = shift_pose(which_arm, response.final_pose, 'z', 0.02, 0.02) # Pull back a bit
# Grab knob -- with feeling!
rospy.wait_for_service('move_gripper')
move_gripper = rospy.ServiceProxy('move_gripper', MoveGripper)
centered = False
shift_distance = 0.02
while not centered:
gripper.findTwoContacts() # grip the knob lightly
balance = gripper.balance.data # get the grip balance
rospy.loginfo(balance)
gripper.open()
if balance == 'CENTERED':
centered = True
elif balance == 'NO_CONTACT':
rospy.logwarn('WARNING: No contact detected -- gripper may not have closed completely!')
calibrate_pressure(request)
else:
# Construct vector by which to shift the gripper
shift = Vector3Stamped()
shift.header.frame_id = 'r_gripper_l_finger_tip_frame'
frame = '/r_wrist_roll_link'
shift.header.stamp = rospy.Time(0)
if balance == 'GO_POSITIVE':
shift.vector.z = shift_distance
elif balance == 'GO_NEGATIVE':
shift.vector.z = shift_distance * -1
listener.waitForTransform(frame, shift.header.frame_id, rospy.Time(0), rospy.Duration(3.0))
shift = listener.transformVector3(frame, shift) # Transform into wrist frame
# Construct current pose of right wrist
wrist_pose = PoseStamped()
wrist_pose.header.frame_id = '/r_wrist_roll_link'
wrist_pose.pose.orientation.w = 1.0
# Command gripper to offset its pose by the shift vector
request = MoveGripperRequest()
request.which_arm = 'right'
request.pose_goal = wrist_pose
request.speed = 0.01
request.goal_offset = shift
response = move_gripper(request)
shift_distance *= 0.50 # reduce shift distance to home in on a good grasp
# After several tries, stop adjusting and try a grasp
if shift_distance < 0.002:
rospy.logwarn('WARNING: could not find a nice center for grasping -- may be suboptimal!')
break
# Grab knob
gripper.findTwoContacts()
gripper.hold(1.0)
# Launch GUI for turning wrist
turn_knob = TurnKnobGUI(sys.argv,
which_arm,
which_arm[0] + '_forearm_cam/image_rect_color')
turn_knob.execute()
del turn_knob
# Finish up
gripper.open()
response = go_to_knob(which_arm, name, standoff, offset, 0.20) # pull away from knob
response = UseKnobResponse()
response.success = True # HACK
return response
