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()