def launch_file(file_path): uuid = rlutil.get_or_generate_uuid(None, False) configure_logging(uuid=uuid) launch_process = parent.ROSLaunchParent(uuid, [file_path]) launch_process.start() launch_process.spin_once() return launch_process
def __init__(self, file_path, relay): ActionBase.__init__(self, relay) launch_file = open(file_path, "r") launch_text = launch_file.read() launch_file.close() launch_text = self._replace_preview_argument(launch_text) self.temp_launch_file = tempfile.NamedTemporaryFile(mode='w+t', delete=False) self.temp_launch_file.write(launch_text) self.temp_launch_file.close() uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) self.listener = roslaunch.pmon.ProcessListener() self.listener.process_died = self._process_died self.launch = roslaunch.parent.ROSLaunchParent(uuid, [self.temp_launch_file.name], process_listeners=[self.listener])
def launch(roslaunch_strs): uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [], roslaunch_strs=roslaunch_strs) launch.start() launch.spin()
def main(): rospy.init_node('rosbridge_system') systemTaskSwitcher = systemTask() pub = pubStates() charge_reset = True pub_charge = rospy.Publisher('/nav_ctrl', NavigationControl, queue_size=10) charge_msg = NavigationControl() charge_msg.control = 1 charge_msg.goal_name = "autocharge_on" # to do: TF_OLD_DATA caused by simulation environment: send an Empty message the topic /reset_time while not rospy.is_shutdown(): # launch slam if systemTaskSwitcher.startSLAM and (systemTaskSwitcher.excutedSLAM == False): if systemTaskSwitcher.slamReset: slam_launch_path = slam_reset_launch_path print( "\033[1;37;42m\t\t\trosbridge_system: SLAM reset mode\t\t\t\033[0m") elif systemTaskSwitcher.slamContinue: slam_launch_path = slam_continue_launch_path print( "\033[1;37;42m\t\t\trosbridge_system: SLAM continue mode\t\t\t\033[0m") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent( uuid, [slam_launch_path]) try: launch.start() time.sleep(3) # enough time for launch to complete print( "\033[1;37;42m\t\t\trosbridge_system: SLAM is launched\t\t\t\033[0m") systemTaskSwitcher.excutedSLAM = True except: systemTaskSwitcher.excutedSLAM = False if systemTaskSwitcher.excutedSLAM: try: # deactivate /base_laser for publish tf between /base_footprint and /base_laser pubTaskSwitch('base_laser', 0, 2) print( "\033[1;37;44m\t rosbridge_system: setting task base_laser False within 2s \033[0m") # deactivate /amcl node for publish tf between /map and /odom dynamicParameterSet('amcl', 'tf_broadcast', False) print( "\033[1;37;44m\t rosbridge_system: setting AMCL tf False \033[0m") # os.system("rosrun dynamic_reconfigure dynparam set /amcl tf_broadcast fasle") except: pass # shutdown slam: (1)record slam pose (2)shutdown # (3)recover tf of amcl and base_laser (4) publish slam pose to /initialpose if (systemTaskSwitcher.startSLAM == False) and systemTaskSwitcher.excutedSLAM: slam_laser_x, slam_laser_y, slam_laser_quat = getAMCLPose( 'map', 'base_laser') print("\033[1;37;44m\t rosbridge_system: getting SLAM pose \033[0m") time.sleep(2) try: dynamicParameterSet('amcl', 'tf_broadcast', True) print( "\033[1;37;44m\t rosbridge_system: setting AMCL tf True \033[0m") pubTaskSwitch('base_laser', 1, 2) print( "\033[1;37;44m\t rosbridge_system: setting task base_laser True within 2s \033[0m") except: pass try: launch.shutdown() time.sleep(3) # enough time for shutdown to complete print( "\033[1;37;41m\t\t\trosbridge_system: SLAM is shutdown\t\t\t\033[0m") systemTaskSwitcher.excutedSLAM = False systemTaskSwitcher.slamReset = False systemTaskSwitcher.slamContinue = False except: systemTaskSwitcher.excutedSLAM = True # publish slam pose to /initialpose for amcl print( "\033[1;37;44m\t rosbridge_system: pulish AMCL pose to /initialpose within 2s \033[0m") pubAMCLPoseAMCLInitial( slam_laser_x, slam_laser_y, slam_laser_quat, 2) print( "\033[1;37;44m\t rosbridge_system: finish SLAM task \033[0m") if systemTaskSwitcher.startShelfDetector and (systemTaskSwitcher.excutedShelfDetector == False): uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_shelf = roslaunch.parent.ROSLaunchParent( uuid, [shelf_launch_path]) try: launch_shelf.start() time.sleep(2) print( "\033[1;37;42m\t\t\trosbridge_system: SHELF_DETECT is launched\t\t\t\033[0m") systemTaskSwitcher.excutedShelfDetector = True except: systemTaskSwitcher.excutedShelfDetector = False if systemTaskSwitcher.startShelfDetector == False and systemTaskSwitcher.excutedShelfDetector: try: launch_shelf.shutdown() time.sleep(2) print( "\033[1;37;41m\t\t\trosbridge_system: SHELF_DETECT is shutdown\t\t\t\033[0m") systemTaskSwitcher.excutedShelfDetector = False except: systemTaskSwitcher.excutedShelfDetector = True if systemTaskSwitcher.battery_percentage < 0.2 and systemTaskSwitcher.navCtrlStatus == 'free' and charge_reset: # pubAutoCharge(1, 'autocharge_on') try: pub_charge.publish(charge_msg) time.sleep(0.2) print('autocharge_on') charge_reset = False except: charge_reset = True if not charge_reset and systemTaskSwitcher.battery_percentage > 0.25: charge_reset = True if systemTaskSwitcher.startDoubleLocalization and (systemTaskSwitcher.excutedDoubleLocalization == False): try: add_dynamic_obstacles_to_launch_file( double_localization_empty_launch, double_localization_generation, systemTaskSwitcher.amcl_x, systemTaskSwitcher.amcl_y, systemTaskSwitcher.amcl_yaw, systemTaskSwitcher.odom_x, systemTaskSwitcher.odom_y, systemTaskSwitcher.odom_yaw) time.sleep(0.1) uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_double_localization = roslaunch.parent.ROSLaunchParent( uuid, [double_localization_generation]) launch_double_localization.start() time.sleep(2) dynamicParameterSet('amcl', 'tf_broadcast', False) print( "\033[1;37;42m\t\t\trosbridge_system: Double Localization is launched\t\t\t\033[0m") print( "\033[1;37;44m\t rosbridge_system: setting AMCL tf False \033[0m") systemTaskSwitcher.excutedDoubleLocalization = True except: systemTaskSwitcher.excutedDoubleLocalization = False if systemTaskSwitcher.startDoubleLocalization == False and systemTaskSwitcher.excutedDoubleLocalization: try: dynamicParameterSet('amcl', 'tf_broadcast', True) print( "\033[1;37;44m\t rosbridge_system: setting AMCL tf True \033[0m") launch_double_localization.shutdown() time.sleep(2) print( "\033[1;37;41m\t\t\trosbridge_system: Double Localization is shutdown\t\t\t\033[0m") systemTaskSwitcher.excutedDoubleLocalization = False except: systemTaskSwitcher.excutedDoubleLocalization = True # publish tasks state pub.publishing() # to do: Inbound TCP/IP connection time.sleep(0.1) try: rospy.spin() except KeyboardInterrupt: print "Shutting down..."
def launch_simulation(sensor_poses=[], target_poses=[]): ''' Simplified pose format: [x,y,z,Yaw]. The poses are lists of 4-vectors. The number of sensors and targets to use is automatically determined by the dimensions of poses passed in. ''' # Using roslaunch api to launch an empty world. rospy.init_node('fim_track_simulation', anonymous=False) uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.scriptapi.ROSLaunch() # Remark: now we are reusing the empty world launch file provided by gazebo_ros to provide a ground plane. More complicated worlds can be used in the future. launch.start() # Launch the turtlesim enviroment. turtlesim_namespace = "/" node = roslaunch.core.Node(package='turtlesim', node_type='turtlesim_node', name='turtlesim_environment', namespace=turtlesim_namespace, output='screen') launch.launch(node) # Start spawning objects into the world. rospy.wait_for_service('{}/spawn'.format(turtlesim_namespace)) rospy.wait_for_service('{}/kill'.format(turtlesim_namespace)) spawn = rospy.ServiceProxy('{}/spawn'.format(turtlesim_namespace), Spawn) kill = rospy.ServiceProxy('{}/kill'.format(turtlesim_namespace), Kill) # Remove the default turtle from the world kill('turtle1'.format(turtlesim_namespace)) # Specify target names and mobile sensor names. target_names = ['target_{}'.format(i) for i in range(len(target_poses))] mobile_sensor_names = [ 'mobile_sensor_{}'.format(i) for i in range(len(sensor_poses)) ] # Spawn the targets, start emitting virtual lights for i, pose in enumerate(target_poses): try: resp = spawn(pose[0], pose[1], pose[2], target_names[i]) virtual_light_node = roslaunch.core.Node( package='fim_track', node_type='virtual_light.py', name='virtual_light_for_{}'.format(target_names[i]), namespace='', args=target_names[i], output='screen') launch.launch(virtual_light_node) except rospy.exceptions.ROSException: print('Object Spawning Error') pass # Spawn the mobile sensors, start receiving virtual lights. for i, pose in enumerate(sensor_poses): try: resp = spawn(pose[0], pose[1], pose[2], mobile_sensor_names[i]) arg_string = "turtlesimPose" + " " + mobile_sensor_names[ i] + " " + " ".join(target_names) virtual_sensor_node = roslaunch.core.Node( package='fim_track', node_type='virtual_sensor.py', name='virtual_sensor_for_{}'.format(mobile_sensor_names[i]), namespace='', args=arg_string, output='screen') launch.launch(virtual_sensor_node) except rospy.exceptions.ROSException: print('Object Spawning Error') pass try: launch.spin() except rospy.exceptions.ROSInterruptException: # After Ctrl+C is pressed. pass
def launch_simulator(launch_file_path): # rospy.init_node('humanoid', anonymous=True) uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [launch_file_path]) launch.start()
#################################################################################################################### urdf_file = open(relaxedIK.vars.urdf_path, 'r') urdf_string = urdf_file.read() rospy.set_param('robot_description', urdf_string) js_pub = rospy.Publisher('joint_states', JointState, queue_size=5) ee_pose_goals_pub = rospy.Publisher('/relaxed_ik/ee_pose_goals', EEPoseGoals, queue_size=3) tf_pub = tf.TransformBroadcaster() rospy.sleep(0.3) # Don't change this code ########################################################################################### uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch_path = os.path.dirname( __file__) + '/../launch/robot_state_pub.launch' launch = roslaunch.parent.ROSLaunchParent(uuid, [launch_path]) launch.start() #################################################################################################################### rospy.sleep(1.0) counter = 0.0 stride = 0.08 idx = 0 while not rospy.is_shutdown(): c = math.cos(counter) s = 0.2 num_ee = relaxedIK.vars.robot.numChains
def launch_new_process(self): uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) self.launch = roslaunch.parent.ROSLaunchParent(uuid, [self.path]) print('Launching launch file at: {}'.format(self.path)) self.launch.start()
def check_buttons(): global buttons_array global launch global calibrate_complete global collect_complete global send_complete global velocity_paused # Check abort button if buttons_array[3] == 1: rospy.logerr("STOP BUTTON SELECTED, blocking velocity commands...") os.system("rosnode kill safety_node") rospy.sleep(1) # Sleep for 1 second to allow time for node to shutdown print "" print "Press %s to continue following waypoints" % continue_btn_sym print "" velocity_paused = True elif buttons_array[4] == 1 and velocity_paused == True: rospy.loginfo("continuing to follow wapoints...") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [location_safety_node]) launch.start() velocity_paused = False # Start collecting goals if buttons_array[0] == 1: while buttons_array[0] == 1: # Wait for button to be released pass rospy.loginfo("Starting collect_goals.launch...") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [location_collect]) launch.start() # Start sending goals elif buttons_array[1] == 1: while buttons_array[1] == 1: pass rospy.loginfo("Starting send_goals.launch...") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [location_send]) launch.start() # Start Heading Calbration elif buttons_array[2] == 1: while buttons_array[2] == 1: pass rospy.loginfo("Starting heading_calibration.launch...") uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [location_calibrate]) launch.start() # Check if end notice has been published by other nodes if (calibrate_complete or collect_complete or send_complete): rospy.sleep( 2) # Sleep for 2 seconds to allow time for other nodes to shutdown launch.shutdown() print_instructions() # Reset all parameters calibrate_complete = False collect_complete = False send_complete = False
def __init__(self): rospy.on_shutdown(self._shutdown) # create roslaunch handles uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) self._launch_tuck_robot = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/robot/tuck_robot.launch' ]) self._launch_tuck_robot2 = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/robot/tuck_robot.launch' ]) self._launch_move_to_origin = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/dance_invitation/move_to_origin.launch' ]) self._launch_move_to_origin2 = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/dance_invitation/move_to_origin.launch' ]) self._launch_face_detection = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/face_tracking/face_tracking.launch' ]) self._launch_move_closer = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/dance_invitation/move_closer.launch' ]) self._launch_follow_me_pose = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/follow_me/follow_me_pose.launch' ]) self._launch_invitation_answer = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/dance_invitation/invitation_answer.launch' ]) self._launch_follow_me_activation = roslaunch.parent.ROSLaunchParent( uuid, [ rospkg.RosPack().get_path('movo_demos') + '/launch/follow_me/follow_me_activation.launch' ]) self._init_robot_obj = InitRobot(self._launch_tuck_robot, self._launch_move_to_origin, 'INIT_ROBOT') self._search_face_obj = SearchFace(self._launch_face_detection, self._launch_move_closer) self._move_closer_obj = MoveCloser(self._launch_move_closer) self._dance_request_obj = DanceRequest(self._launch_follow_me_pose, self._launch_invitation_answer, self._launch_face_detection) self._lets_dance_obj = LetsDance(self._launch_follow_me_activation, self._launch_face_detection) self._ending_back_obj = InitRobot(self._launch_tuck_robot2, self._launch_move_to_origin2, 'ENDING_BACK') # construct state machine self._sm = self._construct_sm() rospy.loginfo('State machine Constructed') # Run state machine introspection server for smach viewer self._intro_spect = smach_ros.IntrospectionServer( 'dance_invitation', self._sm, '/DANCE_INVITATION') self._intro_spect.start() outcome = self._sm.execute() rospy.spin() self._intro_spect.stop()
def generate_data(self): world_size = np.array(rospy.get_param('~world_size', [100., 100.])) world_origin = np.array(rospy.get_param('~world_origin', [0., 0.])) goal_distance = rospy.get_param('~goal_distance', 30) timeout = rospy.get_param('~timeout', 60) ros_pack = rospkg.RosPack() sim_launch_path = ros_pack.get_path('sim_launch') bag_name = sim_launch_path + '/data/' + time.strftime( "%Y%m%d-%H%M%S") + '.bag' # launch unity binary_path = ros_pack.get_path( 'sim_launch') + '/sim_binary/arl_sim_binary.x86_64' with open(os.devnull, 'w') as fp: self.proc = subprocess.Popen([binary_path], stdout=fp) # roslaunch octomap uuid2 = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid2) sim_launch_launch_path = sim_launch_path + '/launch/gt_octomap_only.launch' roslaunch_file2 = [(roslaunch.rlutil.resolve_launch_arguments( [sim_launch_launch_path])[0], [])] self.octomap_launch = roslaunch.parent.ROSLaunchParent( uuid2, roslaunch_file2) self.octomap_launch.start() rospy.logwarn("started octomap launch") rospy.sleep(3) # generate random start points start_pt = self.get_unoccupied_point(world_origin, (world_size - goal_distance)) yaw = np.random.rand() * 2 * np.pi rospy.logwarn('start location ' + str(start_pt[0]) + ',' + str(start_pt[1])) # roslaunch sim_launch uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) sim_launch_launch_path = sim_launch_path + '/launch/generate_data_bags_helper.launch' cli_args = [ sim_launch_launch_path, 'x:=' + str(start_pt[0]), 'y:=' + str(start_pt[1]), 'yaw:=' + str(yaw), 'bag_name:=' + bag_name ] roslaunch_args = cli_args[1:] roslaunch_file = [ (roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args) ] self.launches = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) self.launches.start() rospy.logwarn("started everything else launch") # Call action server client = actionlib.SimpleActionClient('move_base', MoveBaseAction) client_gt = actionlib.SimpleActionClient( 'ground_truth_planning/move_base', MoveBaseAction) start_time = rospy.Time.now() while not rospy.is_shutdown() and not client.wait_for_server( rospy.Duration(1)): rospy.logwarn("NO ACTION SERVER - waiting") rospy.sleep(.1) if rospy.Time.now() - start_time > rospy.Duration(30): rospy.logerr("Timeout on action server") return while not rospy.is_shutdown() and not client_gt.wait_for_server( rospy.Duration(1)): rospy.logwarn("NO ground truth ACTION SERVER - waiting") rospy.sleep(.1) if rospy.Time.now() - start_time > rospy.Duration(30): rospy.logerr("Timeout on action server") return goal = MoveBaseGoal() # TODO also randomize orientation goal_pt = self.get_unoccupied_point( start_pt, goal_distance) # generate random goal point goal.target_pose.pose.orientation.w = 1 goal.target_pose.pose.position.x = goal_pt[0] goal.target_pose.pose.position.y = goal_pt[1] rospy.logwarn('goal location ' + str(goal.target_pose.pose.position.x) + ',' + str(goal.target_pose.pose.position.y)) goal.target_pose.header.frame_id = 'world' client.send_goal(goal) client_gt.send_goal(goal) rospy.loginfo("Sent goal") rospy.logwarn("Waiting for Action Server result " + str(timeout) + "s") start_time = rospy.Time.now() result = False while not rospy.is_shutdown(): dur = rospy.Time.now() - start_time if self.status == GoalStatus.SUCCEEDED: result = True rospy.logwarn("Action server succeeded") break if 2 <= self.status <= 5 or 2 <= self.status_gt <= 5: if dur < rospy.Duration(10): result = False rospy.logerr("Action server Failed") break else: rospy.logwarn("Action server Failed but not deleting bag.") result = True break if dur > rospy.Duration(timeout): result = True rospy.logerr("Timeout, not deleting bag") break rospy.sleep(.1) if not result: rospy.logwarn("Deleting bag") if os.path.exists(bag_name): os.remove(bag_name) if os.path.exists(bag_name + '.active'): os.remove(bag_name + '.active')
def runTest(self): # Start roscore import subprocess roscore = subprocess.Popen('roscore') time.sleep(1) # Get the path to talos_data arospack = rospkg.RosPack() talos_data_path = arospack.get_path('talos_data') # Start talos_gazebo rospy.init_node('starting_talos_gazebo', anonymous=True) uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) cli_args = [ talos_data_path + '/launch/talos_gazebo_alone.launch', 'world:=empty_forced', 'enable_leg_passive:=false', ] roslaunch_args = cli_args[1:] roslaunch_file = [ (roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args) ] launch_gazebo_alone = roslaunch.parent.ROSLaunchParent( uuid, roslaunch_file) launch_gazebo_alone.start() rospy.loginfo("talos_gazebo_alone started") rospy.wait_for_service("/gazebo/pause_physics") gazebo_pause_physics = rospy.ServiceProxy('/gazebo/pause_physics', Empty) gazebo_pause_physics() time.sleep(5) # Spawn talos model in gazebo launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent( uuid, [talos_data_path + '/launch/talos_gazebo_spawn_hs.launch']) # launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(uuid, # [talos_data_path+'/launch/talos_gazebo_spawn_hs_wide.launch']) launch_gazebo_spawn_hs.start() rospy.loginfo("talos_gazebo_spawn_hs started") rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters") time.sleep(5) gazebo_unpause_physics = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) gazebo_unpause_physics() # Start roscontrol launch_bringup = roslaunch.parent.ROSLaunchParent( uuid, [talos_data_path + '/launch/talos_bringup.launch']) launch_bringup.start() rospy.loginfo("talos_bringup started") # Start sot roscontrol_sot_talos_path = arospack.get_path('roscontrol_sot_talos') launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent( uuid, [ roscontrol_sot_talos_path + '/launch/sot_talos_controller_gazebo.launch' ]) launch_roscontrol_sot_talos.start() rospy.loginfo("roscontrol_sot_talos started") time.sleep(5) pkg_name = 'talos_integration_tests' executable = 'test_kine.py' node_name = 'test_kine_py' test_kine_node = roslaunch.core.Node(pkg_name, executable, name=node_name) launch_test_kine = roslaunch.scriptapi.ROSLaunch() launch_test_kine.start() test_kine_process = launch_test_kine.launch(test_kine_node) r = rospy.Rate(10) while not rospy.is_shutdown(): # Test if sot_talos_balance is finished or not if not test_kine_process.is_alive(): self.validation_through_gazebo() # If it is finished then find exit status. if test_kine_process.exit_code != 0: exit_status = "test_sot_talos_balance failed" else: exit_status = None print("Stopping SoT") launch_roscontrol_sot_talos.shutdown() print("Stopping Gazebo") launch_gazebo_alone.shutdown() # Shutdown the node rospy.signal_shutdown(exit_status) # Terminate the roscore subprocess print("Stop roscore") roscore.terminate() r.sleep()
def __init__(self): #clean up previous process os.system("killall rosmaster gzserver gzclient") if rosgraph.is_master_online(): # Checks the master uri and results boolean (True or False) print 'ROS MASTER is active' nodes = rosnode.get_node_names() if "/rviz" in nodes: print("Rviz active") rvizflag=" rviz:=false" else: rvizflag=" rviz:=true" #start ros impedance controller uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) self.launch = roslaunch.parent.ROSLaunchParent(uuid, [os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller.launch"]) #only available in ros lunar # roslaunch_args=rvizflag # self.launch = roslaunch.parent.ROSLaunchParent(uuid, [os.environ['LOCOSIM_DIR'] + "/ros_impedance_controller/launch/ros_impedance_controller_stdalone.launch"],roslaunch_args=[roslaunch_args]) self.launch.start() ros.sleep(4.0) threading.Thread.__init__(self) # instantiating objects self.robot_name = ros.get_param('/robot_name') self.ros_pub = RosPub(self.robot_name,True) self.joint_names = "" self.u = Utils() self.kin = HyQKinematics() self.comPoseW = np.zeros(6) self.baseTwistW = np.zeros(6) self.stance_legs = np.array([True, True, True, True]) self.q = np.zeros(12) self.qd = np.zeros(12) self.tau = np.zeros(12) self.q_des =np.zeros(12) self.qd_des = np.zeros(12) self.tau_ffwd =np.zeros(12) self.b_R_w = np.eye(3) self.grForcesW = np.zeros(12) self.basePoseW = np.zeros(6) self.J = [np.eye(3)]* 4 self.wJ = [np.eye(3)]* 4 self.sub_contact = ros.Subscriber("/"+self.robot_name+"/contacts_state", ContactsState, callback=self._receive_contact, queue_size=100) self.sub_pose = ros.Subscriber("/"+self.robot_name+"/ground_truth", Odometry, callback=self._receive_pose, queue_size=1) self.sub_jstate = ros.Subscriber("/"+self.robot_name+"/joint_states", JointState, callback=self._receive_jstate, queue_size=1) self.pub_des_jstate = ros.Publisher("/command", JointState, queue_size=1) # freeze base and pause simulation service self.reset_world = ros.ServiceProxy('/gazebo/set_model_state', SetModelState) self.reset_gravity = ros.ServiceProxy('/gazebo/set_physics_properties', SetPhysicsProperties) self.pause_physics_client = ros.ServiceProxy('/gazebo/pause_physics', Empty) self.unpause_physics_client = ros.ServiceProxy('/gazebo/unpause_physics', Empty) # Loading a robot model of robot (Pinocchio) self.robot = getRobotModel(self.robot_name) #send data to param server self.verbose = True #this can be read from config file self.u.putIntoGlobalParamServer("verbose", self.verbose)
def execute(self, userdata): # function called when exiting from the node time.sleep(1) tmp = userdata.find_dictionary_in ball_color = '' # Launch the eplore-lite package uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) roslaunch.configure_logging(uuid) launch = roslaunch.parent.ROSLaunchParent(uuid, [self.path]) launch.start() time.sleep(5) # Timeout condition initialization now = rospy.get_rostime() if self.first == True: self.first = False ## @private End time of time-out condition self.then = rospy.get_rostime() # If no ball has been seen after 60 secs shutdown explore-lite pkg while (now.secs - self.then.secs) < 60: for id, info in tmp.items(): if (id == ball_state.color): ball_color = id if ball_state.state == True and ball_color: if tmp[ball_color]['seen'] == False: ball_color = '' # if this is the ball i was looking for --> 'gotoTrack' and then --> 'gotoPlay' if ball_state.color == userdata.find_color_ball_in: launch.shutdown() client.cancel_all_goals() self.first = True userdata.find_dictionary_out = tmp userdata.find_next_out = 'gotoPlay' return 'gotoTrack' # else 'gotoTrack' and then --> 'gotoFind' else: launch.shutdown() client.cancel_all_goals() userdata.find_next_out = 'gotoFind' userdata.find_dictionary_out = tmp return 'gotoTrack' ball_color = '' # Update timeout condition now = rospy.get_rostime() # If the robot was not able to find the ball it was looking for self.first = True client.cancel_all_goals() launch.shutdown() print( "The robot was not able to find the ball within 60 secs. It will go back to the user!" ) userdata.find_dictionary_out = tmp return 'gotoPlay' # Wait for ctrl-c to stop the application rospy.spin()