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)