def _feedback_cb(self, msg):
"""
Monitor action feedback
"""
if msg.status == 'action out of duration':
self.cancel_plan(Empty())
rospy.sleep(self._rate.sleep_dur * 30)
self.resume_plan(Empty())
def reset(self, randomize=True):
"""
Shuffles objects at all environment locations.
"""
if randomize:
self.objects = np.random.randint(0, self.num_categories,
self.num_objects)
if not self.robot_server: self.reset_current_location(Empty())
else: self.robot_server.reset_current_location(Empty())
self.current_location = 0
self.current_state = 'init'
def joy_callback(self, msg):
# Decode joystick message
# Decode buttons message
buttons = msg.buttons
# Check if pressed the centre button and send a test message
if buttons[self.b_message]:
req = EyeMessage.Request()
req.display = req.BOTH
req.type = req.WIDE
req.timeout = 3
req.message = ["Hello", "World"]
self.get_logger().info(f"request service message")
future = self.cli_message.call_async(req)
return
elif buttons[self.b_diagnostic]:
req = Empty.Request()
self.get_logger().info(f"request service diagnostic")
future = self.cli_diagnostic.call_async(req)
return
# Decode axes message
axes = msg.axes
eyes_x = axes[self.axes_x] * 100.
eyes_y = axes[self.axes_y] * 100.
# Check if the message is the same, if true return and don't send the same message
if (eyes_x
== self.eyes_msg.position.x) and (eyes_y
== self.eyes_msg.position.y):
return
# Read axis
self.eyes_msg.position.x = eyes_x
self.eyes_msg.position.y = eyes_y
# Wrap to Eyes message
self.publisher_.publish(self.eyes_msg)
def service_client(self):
client = self.create_client(Empty, "reset")
while client.wait_for_service(0.25) == False:
self.get_logger().warn("wating for server")
request = Empty.Request()
futur_obj = client.call_async(request)
def reset(self):
"""
Reset the agent for a particular experiment condition.
"""
self.iterator = 0
if self.reset_jnts is True:
# reset simulation
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
self.ros_clock = rclpy.clock.Clock().now().nanoseconds
self.target_position = np.random.uniform(-1.0, 1.0, 3)
# Take an observation
self.ob = self.take_observation()
# Return the corresponding observation
#TODO set new target
return self.ob
def MoveKeys(self, req):
action = req.action
# Open and close
if action == 'o':
msg = close()
self.CloseGripper(msg)
return True
if action == 'p':
msg = Empty()
self.OpenGripper(msg)
return True
# Start recording
if action == 'u':
self.record_trigger = True
self.memory = []
self.T = rospy.get_time()
return True
# Stop recording
if action == 'j':
self.record_trigger = False
with open(self.path + 'modelO_actionSeq_d_roll_' + str(np.random.randint(100000)) + '.pkl', 'wb') as f:
pickle.dump(self.memory, f)
return True
if np.any(action == np.array(['n','m'])):
if action == 'n':
self.df += 1.0
print "Increased velocity by 1"
if action == 'm':
self.df -= 1.0
print "Decreased velocity by 1"
return False
def reset(self):
# reset environment
os.system('rosservice call /gazebo/reset_world "{}"')
# set up the odometry reset publisher
reset_odom = rospy.Publisher('/mobile_base/commands/reset_odometry',
Empty,
queue_size=10)
# reset odometry (these messages take a few iterations to get through)
timer = time()
while time() - timer < 0.25:
reset_odom.publish(Empty())
laser_scan_data = None
# make sure laser scan values exist
while laser_scan_data is None:
try:
laser_scan_data = rospy.wait_for_message('scan',
LaserScan,
timeout=5)
except:
pass
# States last 4 elements are important -4 is heading, -3 is current distance, -2 is obstacle min range, and the -1 is obstacle angle.
# if has reached a previous goal, set a new goal
if self.initGoal:
self.goal_x, self.goal_y = self.respawn_goal.getPosition()
self.initGoal = False
self.goal_distance = self.getGoalDistace()
state, done = self.getState(laser_scan_data)
return np.asarray(state)
def reset(self):
"""
Reset the agent for a particular experiment condition.
"""
self.iterator = 0
if self.reset_jnts:
# reset simulation
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('/reset_simulation service not available, waiting again...')
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
self.ros_clock = rclpy.clock.Clock().now().nanoseconds
# delete entity
while not self.delete_entity_cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('/reset_simulation service not available, waiting again...')
req = DeleteEntity.Request()
req.name = "target"
delete_future = self.delete_entity_cli.call_async(req)
rclpy.spin_until_future_complete(self.node, delete_future)
self.spawn_target()
# Take an observation
obs = self.take_observation()
# Return the corresponding observation
return obs
def reset(self):
print("reset simulation")
# future = self.reset_srv.call_async(Empty.Request())
self.ball_hit_ground = False
self.ball_hit_location = None
self.ball_xyz = self.prev_ball_xyz = None
self.upper_link_xyz = None
req = Trigger.Request()
future = self.atach_srv.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# import pdb; pdb.set_trace()
self.ball_detached = False
req = Empty.Request()
future = self.unpause_srv.call_async(req)
rclpy.spin_until_future_complete(self.node, future)
# rclpy.spin_until_future_complete(self.node, future)
# self.spawn_srv = self.node.create_client(SpawnModel, '/gazebo/spawn_sdf_model')
# self.delete_srv = self.node.create_client(DeleteModel, '/gazebo/delete_model')
# self.reset_srv = self.node.create_client(Empty, "/gazebo/reset_simulation")
# self.com_srv = self.node.create_client(GetLinkState, "/gazebo/get_link_state")
# self.link_states_sub = self.node.create_subscription(LinkStates, "/gazebo/link_states", self.link_states_cb)
# self.joint_states_sub = self.node.create_subscription(JointState, "/joint_states", self.joint_states_cb)
# self.step_srv = self.node.create_client(Trigger, "/roboy/simulation/step")
# self.detach_srv = self.node.create_client(Trigger, "/roboy/simulation/joint/detach")
# self.command_pub = self.node.create_publisher(MotorCommand, "/roboy/middleware/MotorCommand")
# self.step_pub = self.node.create_publisher(Int32, "/roboy/simulation/step")
#
# self.detach_pub = self.node.create_publisher(Bool, "/roboy/simulation/joint/detach")
# rclpy.spin_until_future_complete(self.node, future)
print("done")
def reset_sim(self) -> None:
while not self._reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/reset_simulation service not available, waiting again...')
reset_future = self._reset_sim.call_async(Empty.Request())
# print("Resetting simulation")
rclpy.spin_until_future_complete(self.node, reset_future)
async def callback(msg):
nonlocal node, client_gp, client_rs
if msg.data == service.get(
'show_robot_position'): # 'get_position' key
req = GetPosition.Request()
future = client_gp.call_async(req)
node.get_logger().info('calling get_position service.')
try:
result = await future
except Exception as e:
node.get_logger().info(
'get_position service call failed: {}'.format(e))
else:
node.get_logger().info('get_position service call succeeded.')
node.get_logger().info('Robot at {} to the {}'.format(
(result.x, result.y), result.head))
elif msg.data == service.get('reset_robot'): # 'reset' key
req = Empty.Request()
future = client_rs.call_async(req)
node.get_logger().info('calling reset service.')
try:
result = await future
except Exception as e:
node.get_logger().info(
'Reset service call failed: {}'.format(e))
else:
node.get_logger().info('Reset service call succeeded.')
def reset(self):
"""
Reset the agent for a particular experiment condition.
"""
self.unpause_physics()
self.iterator = 0
if self.reset_jnts is True:
msg = Float32()
for leg in ['lf', 'lm', 'lr', 'rf', 'rm', 'rr']:
for limb in ['c1', 'thigh', 'tibia']:
msg.data = float(0.0)
self._pubs[leg + "_" + limb].publish(msg)
# reset simulation
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/reset_simulation service not available, waiting again...'
)
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
time.sleep(0.2)
self.ros_clock = rclpy.clock.Clock().now().nanoseconds
# Take an observation
obs = self.take_observation(None)
self.pause_physics()
# Return the corresponding observation
return obs
def __init__(self):
#Inheriting "Node" constructor
super().__init__("turtleDataPubNode")
#Counter used to get csv data
self.dataCounter = 1
#Creating turtle speed publisher
self.turtleSpeedPub = self.create_publisher(Twist, "/turtle1/cmd_vel",
10)
#Creating turtle speed subscriber
self.turtleSpeedSub = self.create_subscription(Pose, "/turtle1/pose",
self.sub_callBack, 10)
#Creating reset client
self.turtleResetCli = self.create_client(Empty, "/reset")
#Creating timer with 1Hz rate
self.create_timer(1, self.timer_callBack)
#Opening turtle commands csv file
turtleData_file = open("turtle_commands.csv", "r")
#Reading file data
fileData = turtleData_file.read()
#Getting file data without new lines
self.fileData_commas = fileData.split("\n")
#Creating Reset request
self.resetReq = Empty.Request()
def reset_simulation(self):
req = Empty.Request()
while not self.reset_simulation_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().info('service not available, waiting again...')
self.reset_simulation_client.call_async(req)
def unpause_sim(self) -> None:
while not self._physics_unpause_client.wait_for_service(
timeout_sec=1.0):
self.node.get_logger().info(
'/unpause_physics service not available, waiting again...')
unpause_future = self._physics_unpause_client.call_async(
Empty.Request())
rclpy.spin_until_future_complete(self.node, unpause_future)
def __init__(self):
super().__init__("mode_manager_alive_checker")
self._cli = self.create_client(Empty, '/check_alive_mode_manager')
self._req = Empty.Request()
while not self._cli.wait_for_service(timeout_sec=1.0):
self.get_logger().error("Please run splash server. waiting for 1 second...")
self.create_timer(1, self._check_alive_mode_manager)
self.absence = False
def get_state(self):
state = list()
state.append(float(self.goal_distance))
state.append(float(self.goal_angle))
state.append(float(self.min_obstacle_distance))
state.append(float(self.min_obstacle_angle))
self.local_step += 1
# Succeed
if self.goal_distance < 0.20: # unit: m
print("Goal! :)")
self.succeed = True
self.done = True
self.cmd_vel_pub.publish(Twist()) # robot stop
self.local_step = 0
req = Empty.Request()
while not self.task_succeed_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().info(
'service not available, waiting again...')
self.task_succeed_client.call_async(req)
# Fail
if self.min_obstacle_distance < 0.13: # unit: m
print("Collision! :(")
self.fail = True
self.done = True
self.cmd_vel_pub.publish(Twist()) # robot stop
self.local_step = 0
req = Empty.Request()
while not self.task_fail_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'service not available, waiting again...')
self.task_fail_client.call_async(req)
if self.local_step == 500:
print("Time out! :(")
self.done = True
self.local_step = 0
req = Empty.Request()
while not self.task_fail_client.wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'service not available, waiting again...')
self.task_fail_client.call_async(req)
return state
def env_make(self):
while not self.make_environment_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().warn(
'Environment make client failed to connect to the server, try again ...'
)
self.make_environment_client.call_async(Empty.Request())
def reset(self):
self.state.stacked_reward = 0
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.get_logger().info(
'/reset_simulation service not available, waiting again...')
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self, reset_future)
def set_init_pos(self):
self.error = deque([1, 100], 2)
self.distancia = deque([1]*100, 100)
self._is_ready = False
r = rospy.Rate(10)
while not self._is_ready:
r.sleep()
return Empty()
def __init__(self, callbacks):
smach.State.__init__(self,
outcomes=['done4', 'push', 'box1', 'return'])
self.callbacks = callbacks
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
#self.client.wait_for_server()
self.goal = MoveBaseGoal()
self.clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps',
Empty())
def __init__(self):
super().__init__("turtleReset_node")
self.create_subscription(Pose, "turtle1/pose", self.turtleSub_callBack,
10)
self.turtleReset_client = self.create_client(Empty, "reset")
self.turtleReset_request = Empty.Request()
self.current_x = 0.0
self.current_y = 0.0
def reset(self):
self.scan_msg_received = False
self.stop_action
while not self.reset_world.wait_for_service(timeout_sec=1.0):
print('Reset world service is not available...')
self.reset_world.call_async(Empty.Request())
while not self.reset_simulation.wait_for_service(timeout_sec=1.0):
print('Reset simulation service is not available...')
self.reset_simulation.call_async(Empty.Request())
self.laser_scan_range = [0] * 360
self.states_input = [3.5] * 8
while not self.scan_msg_received and rclpy.ok():
sleep(0.1)
self.collision = False
self.done = False
self.bonous_reward = 0
return self.states_input
def reset_simulation(self):
reset_req = Empty.Request()
self.IndexCounter = 0
# check connection to the service server
while not self.reset_simulation_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().warn(
'service for reset_simulation is not available, waiting ...')
self.reset_simulation_client.call_async(reset_req)
def main():
rclpy.init()
sim_time_param = is_sim_time()
node = rclpy.create_node('unpause_simulation',
allow_undeclared_parameters=True,
automatically_declare_parameters_from_overrides=True,
parameter_overrides=[sim_time_param])
#Default sim_time to True
# sim_time = rclpy.parameter.Parameter('use_sim_time', rclpy.Parameter.Type.BOOL, True)
# node.set_parameters([sim_time])
# if not rclpy.ok():
# rospy.ROSException('ROS master is not running!')
timeout = 0.0
if node.has_parameter('timeout'):
timeout = node.get_parameter('timeout').get_parameter_value().double_value
if timeout <= 0:
raise RuntimeError('Unpause time must be a positive floating point value')
print('Unpause simulation - Time = {} s'.format(timeout))
# start_time = time.time()
# while time.time() - start_time < timeout:
# time.sleep(0.1)
if(timeout > 0):
time.sleep(timeout)
#start_time = time.time()
try:
# Handle for retrieving model properties
unpause = node.create_client(Empty, '/gazebo/unpause_physics')
unpause.wait_for_service(timeout_sec=100)
if(not ready):
raise rclpy.exceptions.InvalidServiceNameException('service is unavailable')
except rclpy.exceptions.InvalidServiceNameException:
print('/gazebo/unpause_physics service is unavailable')
sys.exit()
node.get_logger().info(
'The Gazebo "unpause_physics" service was available {} s after the timeout'.format(timeout))#time.time() - start_time))
req = Empty.Request()
future = unpause.call_async(req)
rclpy.spin_until_future_complete(self, future)
if future.result() is not None:
prop = future.result()
if prop.succes:
print('Simulation unpaused...')
else
node.get_logger().error('Failed to unpaused the simulation')
def collision(self):
# Reset if there is a collision
if self._collision_msg is not None:
while not self.reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info('service not available, waiting again...')
reset_future = self.reset_sim.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, reset_future)
self._collision_msg = None
return True
else:
return False
def __init__(self, *args):
self.current_state = State()
#self.curr_global_pos = NavSatFix()
rospy.init_node('offboard_ctrl')
self.offb_set_mode = SetMode
self.prev_state = self.current_state
# Publishers
self.local_pos_pub = rospy.Publisher('/mavros/setpoint_position/local',
PoseStamped,
queue_size=10)
# Subscribers
self.state_sub = rospy.Subscriber('/mavros/state', State,
self.cb_state)
self.sub_target = rospy.Subscriber('/mavros/offbctrl/target',
PoseStamped, self.cb_target)
# Services
self.arming_client = rospy.ServiceProxy('/mavros/cmd/arming',
CommandBool)
self.takeoff_client = rospy.ServiceProxy('/mavros/cmd/takeoff',
CommandTOL)
self.set_mode_client = rospy.ServiceProxy('/mavros/set_mode', SetMode)
## Create services
self.setpoint_controller_server()
# Init msgs
self.target = PoseStamped()
self.target.pose.position.x = 0
self.target.pose.position.y = 0
self.target.pose.position.z = 2
self.last_request = rospy.get_rostime()
self.state = "INIT"
self.rate = rospy.Rate(20.0) # MUST be more then 2Hz
self.t_run = threading.Thread(target=self.navigate)
self.t_run.start()
print(">> SetPoint controller is running (Thread)")
# Spin until the node is stopped
time.sleep(5)
tmp = Empty()
self.switch2offboard(tmp)
rospy.spin()
def reset(self):
"""
Reset the agent for a particular experiment condition.
"""
self.iterator = 0
if self.reset_jnts is True:
# pause simulation
while not self._physics_pauser.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/pause_physics service not available, waiting again...')
pause_future = self._physics_pauser.call_async(Empty.Request())
print("Pausing physics")
rclpy.spin_until_future_complete(self.node, pause_future)
# reset controllers
while not self._robot_resetter.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/' + self.robot_name +
'/reset service not available, waiting again...')
reset_robot_future = self._robot_resetter.call_async(
Empty.Request())
print("Resetting controller initial positions")
rclpy.spin_until_future_complete(self.node, reset_robot_future)
# reset simulation
while not self._reset_sim.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/reset_simulation service not available, waiting again...'
)
reset_future = self._reset_sim.call_async(Empty.Request())
print("Resetting simulation")
rclpy.spin_until_future_complete(self.node, reset_future)
# unpause simulation
while not self._physics_unpauser.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'/unpause_physics service not available, waiting again...')
unpause_future = self._physics_unpauser.call_async(Empty.Request())
rclpy.spin_until_future_complete(self.node, unpause_future)
print("Unpausing simulation")
def __init__(self, callbacks):
smach.State.__init__(self, outcomes=['success4', 'done4'])
self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
# self.client.wait_for_server()
'''
position:
x: -1.10226629048
y: -2.87965063952
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.548724477596
w: 0.83600325818
'''
'''
position:
x: 0.857642769587
y: -3.15814141971
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.168549594567
w: 0.985693174457
'''
self.target0 = MoveBaseGoal()
self.target0.target_pose.header.frame_id = "map"
self.target0.target_pose.header.stamp = rospy.Time.now()
self.target0.target_pose.pose.position.x = -1.10226629048
self.target0.target_pose.pose.position.y = -2.87965063952
self.target0.target_pose.pose.orientation.x = 0.0
self.target0.target_pose.pose.orientation.y = 0.0
self.target0.target_pose.pose.orientation.z = -0.548724477596
self.target0.target_pose.pose.orientation.w = 0.83600325818
self.target = MoveBaseGoal()
self.target.target_pose.header.frame_id = "map"
self.target.target_pose.header.stamp = rospy.Time.now()
self.target.target_pose.pose.position.x = 0.857642769587
self.target.target_pose.pose.position.y = -3.15814141971
self.target.target_pose.pose.orientation.x = 0.0
self.target.target_pose.pose.orientation.y = 0.0
self.target.target_pose.pose.orientation.z = 0.168549594567
self.target.target_pose.pose.orientation.w = 0.985693174457
self.callbacks = callbacks
self.led_pub = rospy.Publisher('/mobile_base/commands/led1',
Led,
queue_size=1)
self.clear_costmap = rospy.ServiceProxy('/move_base/clear_costmaps',
Empty())
def reset_simulation(self):
"""
Sends a request to the gazebo service to reset the simulaotr
This method mostly will be called upon a task failed request from RL_environment
"""
reset_req = Empty.Request()
# check connection to the service server
while not self.reset_simulation_client.wait_for_service(
timeout_sec=1.0):
self.get_logger().warn(
'service for reset_simulation is not available, waiting ...')
self.reset_simulation_client.call_async(reset_req)
def zero ( thing ) :
global firstFix
firstFix = None
return Empty._response_class()
