def _post_execute(self, result):
self._stop_requested = False
if not rospy.is_shutdown():
self._dynamic_reconfigure_client.update_configuration(
{'enabled': 'False'})
self._publishers['result'].publish(std_msgs.Bool(result))
self._running = False
def target_set_xy(message):
global vehicle, is_run, then, curr_alt, prev_val_read, prev_time, start_yaw
# initialising variables
curr_alt = vehicle.location.global_relative_frame.alt
prev_val_read = vehicle.location.global_relative_frame.alt
prev_time = time.time()
then = time.time()
print("callback started")
curr_time = time.time()
print(curr_alt)
alt_data = get_alt(prev_val_read, curr_time - prev_time, curr_alt, vehicle)
prev_val_read = alt_data[1]
prev_time = curr_time
rospy.loginfo(rospy.get_caller_id() + "I heard %s", message)
camera_constant = alt_data[0] / 557
mav_msg = xy_position_control(message[1] * camera_constant,
message[0] * camera_constant, kp_velx,
kp_vely, start_yaw, vehicle)
vehicle.send_mavlink(mav_msg)
now = time.time()
radius = math.sqrt(message.data[0]**2 + message.data[1]**2)
# break loop if it holds altitude for more than 5 secs
if radius < 5:
if now - then > 5:
pub = rospy.Publisher('master/is_run', msg.Bool, queue_size=25)
is_run_obj = msg.Bool(data=is_run)
pub.publish(is_run_obj)
else:
then = now
def run(self):
''' Main loop '''
COUNTER_MAX = 2**32
hb = apmsg.Heartbeat()
healthy = stdmsg.Bool()
r = rospy.Rate(1.0)
while not rospy.is_shutdown():
# Check all safety conditions
ok = self._is_ok()
# Publish a heartbeat, only if all is well
if ok:
self._health_counter = (self._health_counter + 1) % COUNTER_MAX
hb.counter = self._health_counter
self._pub_heartbeat.publish(hb)
# If health has changed, publish to latched topic
if ok != self._health_state:
self._health_state = ok
healthy.data = ok
self._pub_healthy.publish(healthy)
# Sleep so ROS subscribers and services can run
r.sleep()
def listener():
data = msg.Bool(data=True)
pub = rospy.Publisher('chat', msg.Bool, queue_size=10)
rospy.init_node('listener', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub.publish(data)
rospy.Subscriber('chatter', msg.Bool, callback)
rate.sleep()
def listener():
data = msg.Bool(data=False)
pub = rospy.Publisher('master/is_run', msg.Bool, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10)
time.sleep(5)
pub.publish(data)
rospy.loginfo("published!")
exit()
rate.sleep()
def _ros_enable_subscriber(self, msg):
if msg.data:
if not self._is_running():
self._running = True
self._thread = threading.Thread(target=self.execute)
self._thread.start()
else:
if self._is_running():
self._publishers['result'].publish(std_msgs.Bool(False))
self._stop()
if self._thread is not None:
self._thread.join()
self._thread = None
self._stop_requested = False
def _ros_enable_subscriber(self, msg):
if msg.data:
rospy.loginfo('enable ar pair approach')
if not self._is_running():
self._running = True
self._set_target_base_transform(msg.data)
self._tf_thread = threading.Thread(target=self._broadcast_tfs)
self._tf_thread.start()
self._thread = threading.Thread(target=self.execute)
self._thread.start()
else:
rospy.loginfo('disable ar pair approach')
if self._is_running():
self._publishers['result'].publish(std_msgs.Bool(False))
self._stop()
if self._thread is not None:
self._thread.join()
self._thread = None
self._stop_requested = False
def __callback_laser_scan(self, laser_msg):
"""
the callback function for laser_scan
:param:laser_msg: the laser msg we get from robot
:type: sensor_msgs.msgs.laserScan
:return: none
"""
size_laser_points = len(laser_msg.ranges)
angle_count = 0
pub_data = std_msgs.Bool()
pub_data.data = True
for each_point in range(0, size_laser_points, 1):
point_angle = laser_msg.angle_min + angle_count
# this angle between angle_check_min and angle_check_max is the range we need to check
if self.__angle_check_min <= point_angle <= self.__angle_check_max:
if laser_msg.ranges[
each_point] <= self.__obstacle_threshold_dis:
rospy.loginfo('obstacle find!!!!')
self.__puber_stop.publish(pub_data)
return # need not to check out other points
angle_count += laser_msg.angle_increment
self.__puber_start.publish(pub_data)
def __init__(self, debug=False, goal_type='API', state_type='OL'):
self.debug = debug
self.goal_type = goal_type #default API
self.state_type = state_type #default OL = open loop
if self.goal_type == 'trajectory':
super(RolloutExecuter, self).__init__()
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
#vicon subscription and world frame pose dictionary for rewards
self.vicon_sub = rospy.Subscriber('/vicon/marker_new/pose',
geometry_msgs.PoseStamped,
self.vicon_callback)
self.end_effector_pos = None
self.end_effector_desired = dict()
#robot subscription for state information (dictionary of lists indexed by timestep)
self.robot_state_sub = rospy.Subscriber('robot/joint_states',
sensor_msgs.JointState,
self.robot_state_callback)
self.robot_state_current = dict()
self.robot_joint_names = None
self.state_keys = [
'theta_commanded', 'theta_desired', 'theta_measured', 'velocity',
'torque'
]
if self.state_type == 'CL':
self.state_keys.append('world_pose')
#world frame error in current end effector position and desired from parsed file
self.world_error = []
#publish timing topics
self.timing_pub_state = rospy.Publisher('/cycle_bool',
std_msgs.Bool,
queue_size=1)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/cycle_time',
std_msgs.Time,
queue_size=1)
self.timing_msg_time = std_msgs.Time()
#maintain dictionaries of variables to dump at the end or query
self.vicon_dump = []
self.state_dump = dict()
self.action_dump = []
self.reward_dump = []
self.average_error = [0, 0, 0]
#stop flag
self.stop = False
def enableTimerCallback(self):
message = StdMsgs.Bool()
message.data = self.enabled
self.publisherEnableMotors.publish(message)
def create_state(self):
"""Uses data from :py:attr:`recent` to create the state representation.
1. Reads data from the :py:attr:`recent` dictionary.
2. Process data into a format to pass to :doc:`state_representation`.
3. Pass data to :py:func:`~state_representation.StateManager.get_phi`
and :py:func:`~state_representation.StateManager.get_observation`.
Returns:
(numpy array, dict): Feature vector and ancillary state
information.
"""
# bumper constants from
# http://docs.ros.org/hydro/api/kobuki_msgs/html/msg/SensorState.html
bump_codes = [1, 4, 2]
# initialize data
additional_features = set(tools.features.keys() + ['charging'])
sensors = self.features_to_use.union(additional_features)
# read data (used to make phi)
data = {sensor: None for sensor in sensors}
for source in sensors - {'ir', 'core'}:
data[source] = self.read_source(source)
data['ir'] = self.read_source('ir', history=True)[-10:]
data['core'] = self.read_source('core', history=True)
# process data
if data['core']:
bumps = [dat.bumper for dat in data['core']]
data['bump'] = np.sum(
[[bool(x & bump) for x in bump_codes] for bump in bumps],
axis=0, dtype=bool).tolist()
data['charging'] = bool(data['core'][-1].charger & 2)
# enter the data into rosbag
if self.COLLECT_DATA_FLAG:
for bindex in range(len(data['bump'])):
bump_bool = std_msg.Bool()
bump_bool.data = data['bump'][bindex] if data['bump'][
bindex] else False
self.history.write('bump' + str(bindex), bump_bool,
t=self.current_time)
charge_bool = std_msg.Bool()
charge_bool.data = data['charging']
self.history.write('charging', charge_bool,
t=self.current_time)
if data['ir']:
ir = [[0] * 6] * 3
# bitwise 'or' of all the ir data in last time_step
for temp in data['ir']:
a = [[int(x) for x in format(temp, '#08b')[2:]] for temp in
[ord(obs) for obs in temp.data]]
ir = [[k | l for k, l in zip(i, j)] for i, j in zip(a, ir)]
data['ir'] = [int(''.join([str(i) for i in ir_temp]), 2) for
ir_temp in ir]
# enter the data into rosbag
if self.COLLECT_DATA_FLAG:
ir_array = std_msg.Int32MultiArray()
ir_array.data = data['ir']
self.history.write('ir', ir_array, t=self.current_time)
if data['image'] is not None:
# enter the data into rosbag
# image_array = std_msg.Int32MultiArray()
# image_array.data = data['image']
if self.COLLECT_DATA_FLAG:
self.history.write('image', data['image'], t=self.current_time)
# uncompressed image
data['image'] = np.fromstring(data['image'].data,
np.uint8).reshape(480, 640, 3)
# compressing image
if data['cimage'] is not None:
data['image'] = cv2.imdecode(np.fromstring(data['cimage'].data,
np.uint8),
1)
if data['odom'] is not None:
pos = data['odom'].pose.pose.position
lin_vel = data['odom'].twist.twist.linear.x
ang_vel = data['odom'].twist.twist.angular.z
data['odom'] = np.array([pos.x, pos.y, lin_vel, ang_vel])
# enter the data into rosbag
if self.COLLECT_DATA_FLAG:
odom_x = std_msg.Float64()
odom_x.data = pos.x
odom_y = std_msg.Float64()
odom_y.data = pos.y
odom_lin = std_msg.Float64()
odom_lin.data = lin_vel
odom_ang = std_msg.Float64()
odom_ang.data = ang_vel
self.history.write('odom_x', odom_x, t=self.current_time)
self.history.write('odom_y', odom_y, t=self.current_time)
self.history.write('odom_lin', odom_lin, t=self.current_time)
self.history.write('odom_ang', odom_ang, t=self.current_time)
if data['imu'] is not None:
data['imu'] = data['imu'].orientation.z
# TODO: enter the data into rosbag
if 'bias' in self.features_to_use:
data['bias'] = True
data['weights'] = self.gvfs[0].learner.theta if self.gvfs else None
phi = self.state_manager.get_phi(**data)
if 'last_action' in self.features_to_use:
last_action = np.zeros(self.behavior_policy.action_space.size)
last_action[self.behavior_policy.last_index] = True
phi = np.concatenate([phi, last_action])
# update the visualization of the image data
if self.vis:
self.visualization.update_colours(data['image'])
observation = self.state_manager.get_observations(**data)
observation['action'] = self.last_action
if observation['bump']:
# adds a tally for the added cumulant
self.cumulant_counter.value += 1
return phi, observation
def test_msgbool_py():
yield gen_pymsg_test, create(std_msgs.Bool), std_msgs.Bool(data=True), {'data': True}
yield gen_pymsg_test, create(std_msgs.Bool), std_msgs.Bool(data=False), {'data': False}
def on_button_disable_stereo_clicked(self):
disable_stereo_msg = std_msgs.Bool(False)
self._enable_stereo_pub.publish(disable_stereo_msg)
def on_button_enable_stereo_clicked(self):
enable_stereo_msg = std_msgs.Bool(True)
self._enable_stereo_pub.publish(enable_stereo_msg)
def on_button_off_clicked(self):
start_message = std_msgs.Empty()
self._off_pub.publish(start_message)
disarm_message = std_msgs.Bool(False)
self._arm_bridge_pub.publish(disarm_message)
def test_msgbool_py():
yield gen_pymsg_test, RosMsgBool, std_msgs.Bool(data=True), {'data': True}
yield gen_pymsg_test, RosMsgBool, std_msgs.Bool(data=False), {
'data': False
}
def __init__(self):
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#verify joint trajectory action servers are available
l_server_up = self._left_client.wait_for_server(rospy.Duration(10.0))
r_server_up = self._right_client.wait_for_server(rospy.Duration(10.0))
if not l_server_up or not r_server_up:
msg = ("Action server not available."
" Verify action server availability.")
rospy.logerr(msg)
rospy.signal_shutdown(msg)
sys.exit(1)
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#gripper interface - for gripper command playback
self._l_gripper = baxter_interface.Gripper('left', CHECK_VERSION)
self._r_gripper = baxter_interface.Gripper('right', CHECK_VERSION)
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
# Verify Grippers Have No Errors and are Calibrated
if self._l_gripper.error():
self._l_gripper.reset()
if self._r_gripper.error():
self._r_gripper.reset()
if (not self._l_gripper.calibrated()
and self._l_gripper.type() != 'custom'):
self._l_gripper.calibrate()
if (not self._r_gripper.calibrated()
and self._r_gripper.type() != 'custom'):
self._r_gripper.calibrate()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
# Timing offset to prevent gripper playback before trajectory has started
self._slow_move_offset = 0.0
self._trajectory_start_offset = rospy.Duration(0.0)
self._trajectory_actual_offset = rospy.Duration(0.0)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#gripper control rate
self._gripper_rate = 20.0 # Hz
#publish timing topics
self.timing_pub_state = rospy.Publisher('/board_pose/cycle_on',
std_msgs.Bool,
queue_size=0)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/board_pose/cycle_time',
std_msgs.Time,
queue_size=1)
self.timing_msg_time = std_msgs.Time()
def __init__(self, sweep_joint_dict, debug=False, goal_type='API', state_type='OL'):
self.debug = debug
self.goal_type = goal_type #default API
self.state_type = state_type #default OL = open loop
if self.goal_type == 'trajectory':
super(RolloutExecuter, self).__init__()
#create our action server clients
self._left_client = actionlib.SimpleActionClient(
'robot/limb/left/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
self._right_client = actionlib.SimpleActionClient(
'robot/limb/right/follow_joint_trajectory',
FollowJointTrajectoryAction,
)
#param namespace
self._param_ns = '/rsdk_joint_trajectory_action_server/'
#create our goal request
self._l_goal = FollowJointTrajectoryGoal()
self._r_goal = FollowJointTrajectoryGoal()
#gripper goal trajectories
self._l_grip = FollowJointTrajectoryGoal()
self._r_grip = FollowJointTrajectoryGoal()
#limb interface - current angles needed for start move
self._l_arm = baxter_interface.Limb('left')
self._r_arm = baxter_interface.Limb('right')
#flag to signify the arm trajectories have begun executing
self._arm_trajectory_started = False
#reentrant lock to prevent same-thread lockout
self._lock = threading.RLock()
#vicon subscription and world frame pose dictionary for rewards
self.vicon_sub = rospy.Subscriber('/vicon/marker1/pose', geometry_msgs.PoseStamped, self.vicon_callback)
self.end_effector_pos = None
self.end_effector_desired = dict()
#robot subscription for state information (dictionary of lists indexed by timestep)
self.robot_state_sub = rospy.Subscriber('/robot/limb/right/gravity_compensation_torques', baxter_core_msgs.SEAJointState, self.robot_state_callback)
self.robot_state_current = dict()
# self.robot_joint_names = None
self.robot_joint_names = ['right_s0', 'right_s1', 'right_e0', 'right_e1', 'right_w0', 'right_w1', 'right_w2']
self.state_keys = ['theta_commanded', 'theta_measured', 'velocity', 'torque']
if self.state_type == 'CL':
self.state_keys.append('world_pose')
#world frame error in current end effector position and desired from parsed file
self.world_error = []
#publish timing topics
self.timing_pub_state = rospy.Publisher('/cycle_bool', std_msgs.Bool, queue_size = 1)
self.timing_msg_state = std_msgs.Bool()
self.timing_pub_time = rospy.Publisher('/cycle_time', std_msgs.Time, queue_size = 1)
self.timing_msg_time = std_msgs.Time()
#maintain dictionaries of variables to dump at the end or query
self.vicon_dump = []
self.state_dump = dict()
self.action_dump = []
self.reward_dump = []
self.average_error = [0, 0, 0]
#programmed trajectories
self.current = 0
self.trajectory_matrix = None
self.sweep_joint_dict = sweep_joint_dict
self.iterations = self.sweep_joint_dict['iterations']
self.stop = False
self.joint_scale = np.array([1/2.5, 1/2.5, 1.5/2.5, 1.5/2.5, 3/2.5, 2/2.5])
self.joint_shift = np.array([-np.pi/6, -np.pi/6, -np.pi/4, np.pi/4, 0, 0])
self.joint_min = [-np.pi/3, -np.pi/3, -np.pi/2, 0, -np.pi/2, -np.pi/3, -np.pi]
self.joint_max = [0, 0, 0, np.pi/2, np.pi/2, np.pi/3, np.pi]
self.random_walk = OrnsteinUhlenbeckProcess(
dimension=6, num_steps=self.iterations,
seed_vec=self.sweep_joint_dict['fixed'][1:-1])
self.random_trajectory = self.random_walk.make_new_walk(length=self.iterations, scale_vec=self.joint_scale, shift_vec=self.joint_shift)
if self.debug:
print("random walk trajectory: \n")
print("shape: ")
print(self.random_trajectory.shape)
print("------- \n")
print("max: ")
print(np.max(self.random_trajectory, 0))
print("\n")
print("min: ")
print(np.min(self.random_trajectory, 0))
print("\n")
print("------- \n")
self.random_trajectory = np.hstack((self.random_trajectory, np.ones((self.iterations+1, 1))*2.7))
def on_button_arm_bridge_clicked(self):
arm_message = std_msgs.Bool(True)
self._arm_bridge_pub.publish(arm_message)
