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