我们从Python开源项目中，提取了以下36个代码示例，用于说明如何使用std_msgs.msg.Float64()。
def test_publish_to_topics(self): topic_ending = "desired" rospy.logdebug("Sleeping for 5 seconds to let ROS spin up...") rospy.sleep(5) for variable, value in self.variables: # Publish to each variable/desired topic to see if all of the # actuators come on as expected. topic_string = variable + "/" + topic_ending rospy.logdebug("Testing Publishing to " + topic_string) pub_desired = rospy.Publisher(topic_string, Float64, queue_size=10) sub_desired = rospy.Subscriber(topic_string, Float64, self.callback) rospy.sleep(2) print(self.namespace + "/" + topic_string) for _ in range(NUM_TIMES_TO_PUBLISH): pub_desired.publish(value) rospy.sleep(1) rospy.sleep(2) pub_desired.publish(0)
def update_telemetry(navsat_msg, compass_msg): """Telemetry subscription callback. Args: navsat_msg: sensor_msgs/NavSatFix message. compass_msg: std_msgs/Float64 message in degrees. """ try: client.post_telemetry(navsat_msg, compass_msg) except (ConnectionError, Timeout) as e: rospy.logwarn(e) return except (ValueError, HTTPError) as e: rospy.logerr(e) return except Exception as e: rospy.logfatal(e) return
def test_post_telemetry(self): """Tests posting telemetry data through client.""" # Set up test data. url = "http://interop" client_args = (url, "testuser", "testpass", 1.0) with InteroperabilityMockServer(url) as server: # Setup mock server. server.set_root_response() server.set_login_response() server.set_telemetry_response() # Connect client. client = InteroperabilityClient(*client_args) client.wait_for_server() client.login() client.post_telemetry(NavSatFix(), Float64())
def test_telemetry_serializer(self): """Tests telemetry serializer.""" # Set up test data. navsat = NavSatFix() navsat.latitude = 38.149 navsat.longitude = -76.432 navsat.altitude = 30.48 compass = Float64(90.0) data = serializers.TelemetrySerializer.from_msg(navsat, compass) altitude_msl = serializers.meters_to_feet(navsat.altitude) # Compare. self.assertEqual(data["latitude"], navsat.latitude) self.assertEqual(data["longitude"], navsat.longitude) self.assertEqual(data["altitude_msl"], altitude_msl) self.assertEqual(data["uas_heading"], compass.data)
def __init__(self): """ Create HeadLiftJoy object. """ # params self._head_axes = rospy.get_param('~head_axes', 3) self._lift_axes = rospy.get_param('~lift_axes', 3) self._head_button = rospy.get_param('~head_button', 4) self._lift_button = rospy.get_param('~lift_button', 5) # pubs self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1) self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1) # subs self._joy_sub = rospy.Subscriber('joy', Joy, self._joy_cb, queue_size=1)
def __init__(self): # Read Settings self.read_settings() # Init other variables self._num_magnetometer_reads = 0 self._latest_bearings = np.zeros(shape = (self._number_samples_average, 1)) self._received_enough_samples = False # Subscribe to magnetometer topic rospy.Subscriber("magnetic_field", Vector3Stamped, self.magnetic_field_callback) # Publishers self._pub_bearing_raw = rospy.Publisher(rospy.get_name() + '/bearing_raw_deg', Float64, queue_size = 10) self._pub_bearing_avg = rospy.Publisher(rospy.get_name() + '/bearing_avg_deg', Float64, queue_size = 10) self._pub_imu_bearing_avg = rospy.Publisher(rospy.get_name() + '/imu_bearing_avg', Imu, queue_size = 10) if self._verbose: self._pub_mag_corrected = rospy.Publisher(rospy.get_name() + '/mag_corrected', Vector3Stamped, queue_size = 10) rospy.spin()
def __init__(self): # setup CozmoTeleop.settings = termios.tcgetattr(sys.stdin) atexit.register(self.reset_terminal) # vars self.head_angle = STD_HEAD_ANGLE self.lift_height = STD_LIFT_HEIGHT # params self.lin_vel = rospy.get_param('~lin_vel', 0.2) self.ang_vel = rospy.get_param('~ang_vel', 1.5757) # pubs self._head_pub = rospy.Publisher('head_angle', Float64, queue_size=1) self._lift_pub = rospy.Publisher('lift_height', Float64, queue_size=1) self._cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
def listener(): # In ROS, nodes are uniquely named. If two nodes with the same # node are launched, the previous one is kicked off. The # anonymous=True flag means that rospy will choose a unique # name for our 'listener' node so that multiple listeners can # run simultaneously. rospy.init_node('listener', anonymous=True) rospy.Subscriber("/turtlebot_angle", Float64, callback1) rospy.Subscriber("/turtlebot_posex", Float64, callback2) rospy.Subscriber("/turtlebot_posey", Float64, callback3) rospy.Subscriber("/turtlebot_follower/dist_object1", Float64, callback4) rospy.Subscriber("/turtlebot_follower/dist_object2", Float64, callback5) rospy.Subscriber("/turtlebot_follower/dist_object3", Float64, callback6) rospy.Subscriber("/turtlebot_follower/dist_object4", Float64, callback7) #rospy.Subscriber("/turtlebot_follower/dist_object", Float64, callback8) # spin() simply keeps python from exiting until this node is stopped rospy.spin()
def __init__(self): self.joint_name = rospy.get_param("~joint_name") self.command_pub = rospy.Publisher("command", Float64, queue_size=1) self.joint_sub = rospy.Subscriber("joint_state", JointState, self.joint_state_callback, queue_size=1)
def __init__(self): self.joint_name = rospy.get_param("~joint_name") self.joint_state = JointState() self.joint_state.name.append(self.joint_name) self.joint_state.position.append(0.0) self.joint_state.velocity.append(0.0) self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=1) self.command_sub = rospy.Subscriber("command", Float64, self.command_callback, queue_size=1)
def create_persistence_objects( server, environment_id, max_update_interval, min_update_interval ): env_var_db = server[ENVIRONMENTAL_DATA_POINT] for variable in ENVIRONMENT_VARIABLES.itervalues(): variable = str(variable) topic = "{}/measured".format(variable) TopicPersistence( topic=topic, topic_type=Float64, environment=environment_id, variable=variable, is_desired=False, db=env_var_db, max_update_interval=max_update_interval, min_update_interval=min_update_interval )
def filter_topic(src_topic, dest_topic, topic_type): """ Publishes a measured data point given the raw value by applying the EWMA function to the data. :param src_topic: String? - Source topic signal to be filtered :param dest_topic: String? - Output topic to publish new data to :param topic_type: The data type of the topic, aka Float64, Int32, etc :return: sub, pub : subscribed topic (raw measured value) and published topic (filtered (smoothed) value) """ rospy.loginfo("Filtering topic {} to topic {}".format( src_topic, dest_topic )) pub = rospy.Publisher(dest_topic, topic_type, queue_size=10) f = EWMA(0.3) def callback(src_item): value = src_item.data # If the value is our magic number, leave it alone if value in SENTINELS: dest_item = value else: f(src_item.data) dest_item = topic_type(f.average) pub.publish(dest_item) sub = rospy.Subscriber(src_topic, topic_type, callback) return sub, pub
def filter_all_variable_topics(variables): """ Given an iterator publishers, where each publisher is a two-tuple `(topic, type)`, publishes a filtered topic endpoint. """ for env_var in variables: src_topic = "{}/raw".format(env_var) dest_topic = "{}/measured".format(env_var) # Ignore type associated with environmental variable type and # coerce to Float64 # @FIXME this is a short-term fix for preventing boolean values from # being filtered by the EWMA filter. # # Explanation: right now all topics under `/environment/<id>` are # float64 type, with Boolean being 1 or 0. This was judged to be a # simpler architecture at the time. Values from sensors may be any # type, but are coerced to Float64. The same is true for actuators. # However, this assumption breaks down for filtering boolean values, # since the EWMA will produce fractional numbers that don't coerce # correctly back to boolean. # # In future, we should change the architecture of the system to support # standard ros types under `/environment/<id>`. if env_var.type is None or 'boolean' in env_var.type.lower(): forward_topic(src_topic, dest_topic, Float64) else: filter_topic(src_topic, dest_topic, Float64)
def on_data(self, item): curr_time = time.time() value = item.data if value is None or value == self.last_value: return # This is kind of a hack to correctly interpret UInt8MultiArray # messages. There should be a better way to do this if item._slot_types[item.__slots__.index('data')] == "uint8[]": value = [ord(x) for x in value] # Throttle updates by value only (not time) if self.topic_type == Float64 and \ self.last_value is not None and \ self.last_value != 0.0: delta_val = value - self.last_value if abs(delta_val / self.last_value) <= 0.01: return # Save the data point point = EnvironmentalDataPoint({ "environment": self.environment, "variable": self.variable, "is_desired": self.is_desired, "value": value, "timestamp": curr_time }) point_id = gen_doc_id(curr_time) self.db[point_id] = point self.last_value = value
def run(self): """ Run our code """ rospy.loginfo("Start laser test code") #self.blank_image() #for x in range(0, 99): # self.add_point(x * 1.0, x * 1.0, 10) #cv2.imshow("Hackme", self.img) #cv2.waitKey(0) self.joint_subscriber = rospy.Subscriber("/multisense/joint_states", JointState, self.recv_joint_state) self.scan_subscriber = rospy.Subscriber("/multisense/lidar_scan", LaserScan, self.recv_scan) self.spindle = rospy.Publisher("/multisense/set_spindle_speed", Float64, queue_size=10) self.set_x_range(-1.0, 1.0) self.set_y_range(-1.0, 1.0) self.set_z_range(0.0, 2.0) rospy.sleep(0.1) rospy.loginfo("Setting spindle going") self.spindle.publish(Float64(1.0)) #self.zarj.zps.look_down() rospy.loginfo("Spin forever, hopefully receiving data...") while True: rospy.sleep(1.0) #print "Got cloud {}x{}".format(resp.cloud.height, resp.cloud.width) #img = self.create_image_from_cloud(resp.cloud.points) #cv2.destroyAllWindows() #print "New image" #cv2.imshow("My image", img) #cv2.waitKey(1) #print resp #cv_image = self.bridge.imgmsg_to_cv2(resp.cloud, "bgr8") #cv2.imshow("Point cloud", cv_image)
def __init__(self, P=1/30000): self.limb_name = 'left' self.other_limb_name = 'right' self.limb = baxter_interface.Limb(self.limb_name) self.err_pub = rospy.Publisher('/error', Float64, queue_size=1) self.P = P self.kinematics = baxter_kinematics(self.limb_name) self.jinv = self.kinematics.jacobian_pseudo_inverse()
def compute_faii(self): filtered_acc = signal.lfilter(self.b1, self.a1, self.acc, axis=0) self.faii = np.sqrt((filtered_acc**2).sum(axis=1)) # Publish just the last value val = self.faii[-1] self.faii_pub.publish(Float64(val)) if self.recording: t = time() self.data['faii'].append((t, val))
def __init__(self): rospy.init_node('gripper_controller') self.r_pub = rospy.Publisher('gripper_controller/command', Float64, queue_size=10) # subscribe to command and then spin self.server = actionlib.SimpleActionServer('~gripper_action', GripperCommandAction, execute_cb=self.actionCb, auto_start=False) self.server.start() rospy.spin()
def actionCb(self, goal): """ Take an input command of width to open gripper. """ rospy.loginfo('Gripper controller action goal recieved:%f' % goal.command.position) command = goal.command.position # publish msgs rmsg = Float64(command) self.r_pub.publish(rmsg) rospy.sleep(5.0) self.server.set_succeeded() rospy.loginfo('Gripper Controller: Done.')
def __init__(self): self.joint_names = ['base_joint', 'shoulder_joint', 'elbow_joint', 'wrist_flex_joint', 'wrist_rot_joint', 'finger_joint1', 'finger_joint2'] # subscriber for setting joints position self.states_sub = rospy.Subscriber("/as_arm/set_joints_states", JointState, self.callback, queue_size=2) # a list of publisher self.joint_pub = dict() self.joint_pose = dict() for idx, name in enumerate(self.joint_names): pub = rospy.Publisher("/as_arm/joint%d_position_controller/command" % (idx + 1), Float64, queue_size=3) self.joint_pub[name] = pub self.joint_pose[name] = Float64()
def __init__(self, max_random_start, observation_dims, data_format, display, use_cumulated_reward): self.max_random_start = max_random_start self.action_size = 28 self.use_cumulated_reward = use_cumulated_reward self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) #self.dirToTarget = 0 self.robotPose = Pose() self.goalPose = Pose() self.robotRot = 0.0 self.prevDist = 0.0 self.numWins = 0 self.ep_reward = 0.0 self.readyForNewData = True self.terminal = 0 self.sendTerminal = 0 self.minFrontDist = 6 self.r = 1 self.ang = 0 rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Subscribers: rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10) # publishers: self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10) self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10) self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)
def __init__(self, max_random_start, observation_dims, data_format, display, use_cumulated_reward): self.max_random_start = max_random_start self.action_size = 28 self.use_cumulated_reward = use_cumulated_reward self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) #self.dirToTarget = 0 self.robotPose = Pose() self.goalPose = Pose() self.robotRot = 0.0 self.prevDist = 0.0 self.numWins = 0 self.ep_reward = 0.0 self.readyForNewData = True self.terminal = 0 self.sendTerminal = 0 self.minFrontDist = 6 rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Subscribers: rospy.Subscriber('/input_data', stage_message, self.stageCB, queue_size=10) # publishers: self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 10) self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10) self.pub_goal_ = rospy.Publisher("target", Pose, queue_size = 15)
def __init__(self,side,ik=True): limb.Limb.__init__(self,side) self.side=side self.DEFAULT_BAXTER_SPEED=0.3 if not side in ["left","right"]: raise BaxterException,"Error non existing side: %s, please provide left or right"%side self.post=Post(self) self.__stop = False self.simple=self self._moving=False self.moving_lock=Lock() self.ik=ik if self.ik: self.ns = "/ExternalTools/%s/PositionKinematicsNode/IKService"%self.side rospy.loginfo("Waiting for inverse kinematics service on %s..."%self.ns) rospy.wait_for_service(self.ns) self.iksvc = rospy.ServiceProxy(self.ns, SolvePositionIK) rospy.loginfo("Waiting for inverse kinematics service DONE") else: rospy.loginfo("Skipping inverse kinematics service loading") #self.simple_limb_srv = rospy.Service("/simple_limb/"+side,LimbPose,self.__cbLimbPose) self._pub_speed=rospy.Publisher("/robot/limb/%s/set_speed_ratio"%self.side,Float64,queue_size=1) while not rospy.is_shutdown() and self._pub_speed.get_num_connections() < 1: rospy.sleep(0.1) #self.setSpeed(3) # def __cbLimbPose(self,msg): # cmd = msg.command # if cmd == "goToPose": # resp = self.goToPose(msg.pose, msg.speed, msg.position_tolerance, msg.orientation_tolerance, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, cartesian, msg.path_tolerance) # elif cmd=="goToAngles": # resp = self.goToAngles(msg.angles, msg.speed, msg.joint_tolerance_plan, msg.joint_tolerance, msg.speed_tolerance, msg.timeout, msg.path_tolerance) # return LimbPoseResponse(resp)
def setSpeed(self,speed): finalspeed=self.DEFAULT_BAXTER_SPEED*speed if finalspeed>1: finalspeed=1 if finalspeed<0: finalspeed=0 try: self._pub_speed.publish(Float64(finalspeed)) except: pass
def __init__(self, node_name, sub_topic, sub_base_throttle_topic, pub_topic, pub_setpoint_topic, pub_state_topic, pub_throttle_topic, reset_service): self.bridge = CvBridge() self.img_prep = ImagePreparator() self.ipm = InversePerspectiveMapping() # Publisher self.image_pub = rospy.Publisher(pub_topic, Image, queue_size=QUEUE_SIZE) self.setpoint_pub = rospy.Publisher(pub_setpoint_topic, Float64, queue_size=QUEUE_SIZE) self.state_pub = rospy.Publisher(pub_state_topic, Float64, queue_size=QUEUE_SIZE) self.throttle_pub = rospy.Publisher(pub_throttle_topic, Float64, queue_size=QUEUE_SIZE) self.reset_srv = rospy.Service(reset_service, Empty, self.reset_callback) self.reset_tracking = False rospy.init_node(node_name, anonymous=True) self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback) self.base_throttle_sub = rospy.Subscriber(sub_base_throttle_topic, Float64, self.callbackBaseThrottle) # Base Throttle self.base_throttle = rospy.get_param("/autonomous_driving/lane_detection_node/base_throttle", 0.6) # Crop Parameters self.above_value = rospy.get_param("/autonomous_driving/lane_detection_node/above", 0.58) self.below_value = rospy.get_param("/autonomous_driving/lane_detection_node/below", 0.1) self.side_value = rospy.get_param("/autonomous_driving/lane_detection_node/side", 0.3) # Lane Tracking Parameters self.deviation = rospy.get_param("/autonomous_driving/lane_detection_node/deviation", 5) self.border = rospy.get_param("/autonomous_driving/lane_detection_node/border", 0) # Canny Parameters self.threshold_low = rospy.get_param("/autonomous_driving/lane_detection_node/threshold_low", 50) self.threshold_high = rospy.get_param("/autonomous_driving/lane_detection_node/threshold_high", 150) self.aperture = rospy.get_param("/autonomous_driving/lane_detection_node/aperture", 3) # Lane Tracking self.init_lanemodel() rospy.spin()
def __init__(self, node_name, sub_topic, pub_topic, pub_setpoint_topic, pub_state_topic, reset_service): self.bridge = CvBridge() self.init_lanemodel() self.image_pub = rospy.Publisher(pub_topic, Image, queue_size=QUEUE_SIZE) self.setpoint_pub = rospy.Publisher(pub_setpoint_topic, Float64, queue_size=QUEUE_SIZE) self.state_pub = rospy.Publisher(pub_state_topic, Float64, queue_size=QUEUE_SIZE) self.reset_srv = rospy.Service(reset_service, Empty, self.reset_callback) self.reset_tracking = False rospy.init_node(node_name, anonymous=True) self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback) rospy.spin()
def __init__(self, sub_topic, pub_steering_topic, pub_throttle_topic): self.bridge = CvBridge() self.base_throttle = rospy.get_param("/autonomous_driving/object_detection_node/base_throttle", DEFAULT_BASE_THROTTLE) self.b_calc_steering = rospy.get_param("/autonomous_driving/object_detection_node/b_calc_steering", DEFAULT_B_CALC_STEERING) self.debug_flag = rospy.get_param("/autonomous_driving/object_detection_node/debug_flag", DEBUG_FLAG) self.image_sub = rospy.Subscriber(sub_topic, Image, self.callback) self.steering_pub = rospy.Publisher(pub_steering_topic, Float64, queue_size=1) self.throttle_pub = rospy.Publisher(pub_throttle_topic, Float64, queue_size=1) self.roadmask = None rospy.spin()
def magnetic_field_callback(self, magnetometer_msg): # Correct magnetic filed raw_mag = np.array([magnetometer_msg.vector.x, magnetometer_msg.vector.y, magnetometer_msg.vector.z]) # corrected_mag = compensation * (raw_mag - offset) corrected_mag = np.dot(self._calibration_compensation, raw_mag - self._calibration_offset) # compute yaw angle using corrected magnetometer measurements # and ASSUMING ZERO pitch and roll of the magnetic sensor! # adapted from # https://github.com/KristofRobot/razor_imu_9dof/blob/indigo-devel/src/Razor_AHRS/Compass.ino corrected_mag = corrected_mag / np.linalg.norm(corrected_mag) mag_bearing = math.atan2(corrected_mag[1], -corrected_mag[0]) # add declination and constant bearing offset mag_bearing = mag_bearing + self._declination + self._bearing_offset # publish unfiltered bearing, degrees only for debug purposes self._pub_bearing_raw.publish(Float64(math.degrees(mag_bearing))) # compute mean self._latest_bearings[self._num_magnetometer_reads] = mag_bearing self._num_magnetometer_reads += 1 if self._num_magnetometer_reads >= self._number_samples_average: self._num_magnetometer_reads = 0 # delete oldest samples self._received_enough_samples = True if self._received_enough_samples: bearing_avg = self.angular_mean(self._latest_bearings) else: # not enough samples, use latest value bearing_avg = mag_bearing # WARNING: we assume zero roll and zero pitch! q_avg = tf.quaternion_from_euler(0.0, 0.0, bearing_avg); imu_msg = Imu() imu_msg.orientation.w = q_avg[3] imu_msg.orientation.x = q_avg[0] imu_msg.orientation.y = q_avg[1] imu_msg.orientation.z = q_avg[2] self._pub_bearing_avg.publish(Float64(math.degrees(bearing_avg))) self._pub_imu_bearing_avg.publish(imu_msg) # debug if self._verbose: rospy.loginfo("bearing_avg : " + str(math.degrees(bearing_avg)) + " deg") mag_corrected_msg = magnetometer_msg mag_corrected_msg.vector.x = corrected_mag[0] mag_corrected_msg.vector.y = corrected_mag[1] mag_corrected_msg.vector.z = corrected_mag[2] self._pub_mag_corrected.publish(mag_corrected_msg)
def __init__(self): self.recording = False self.names = ('sair', 'sail', 'fai', 'faii') self.data = {n: [] for n in self.names} # TODO hardcoded for left arm # self.acc_sub = rospy.Subscriber( # '/robot/accelerometer/left_accelerometer/state', # Imu, # self.handle_acc) self.sensor_sub = rospy.Subscriber( '/sensor_values', Int32MultiArray, self.handle_sensor) # self.kb_sub = rospy.Subscriber('/keyboard/keyup', # Key, # self.keyboard_cb, queue_size=10) # Visualising the below pulished signals in rqt_plot is recommended self.sai_pub = rospy.Publisher( '/finger_sensor/sai', FingerSAI, queue_size=5) self.fai_pub = rospy.Publisher( '/finger_sensor/fai', FingerFAI, queue_size=5) self.faii_pub = rospy.Publisher( '/finger_sensor/faii', Float64, queue_size=5) self.sensor_values = [] self.sensor_values = deque(maxlen=80) self.acc_t = deque(maxlen=400) self.acc = deque(maxlen=400) # 0.66pi rad/sample (cutoff frequency over nyquist frequency # (ie, half the sampling frequency)). For the wrist, 33 Hz / # (100 Hz/ 2). TODO Double check arguments self.b1, self.a1 = signal.butter(1, 0.66, 'high', analog=False) # 0.5p rad/sample. For the tactile sensor, 5 Hz / (20 Hz / 2). self.b, self.a = signal.butter(1, 0.5, 'high', analog=False)
def step(self, action, is_training): if self.terminal == 0: if action == -1: # Step with random action action = int(random.random()*(self.action_size)) self.actionToVel( action) self.readyForNewData = True if self.display: cv2.imshow("Screen", self.screen) cv2.waitKey(3) dist = np.sqrt( (self.robotPose.position.x - self.goalPose.position.x)**2 + (self.robotPose.position.y - self.goalPose.position.y)**2) #near obstacle penalty factor: #nearObstPenalty = self.minFrontDist - 1.5 #reward = max( 0, ((self.prevDist - dist + 1.8)*3/dist)+min( 0, nearObstPenalty)) #self.prevDist = dist reward = 0 if dist < 0.3: reward += 1 #Select a new goal theta = self.ang*random.random() self.goalPose.position.x = self.r*np.cos( theta) self.goalPose.position.y = self.r*np.sin( theta) self.pub_goal_.publish( self.goalPose) rwd = Float64() rwd.data = 10101.963 self.pub_rew_.publish( rwd) self.numWins += 1 if self.numWins == 99: if self.ang < np.pi: self.r += 1 self.ang += float(int(self.r/20)/10.0) self.r %=20 self.nimWins = 0 self.resetStage() # Add whatever info you want info = "" self.ep_reward += reward if self.terminal == 1: reward = -1 rewd = Float64() rewd.data = self.ep_reward self.pub_rew_.publish( rewd) self.sendTerminal = 1 while( self.readyForNewData == True): pass if self.use_cumulated_reward: return self.screen, self.ep_reward, self.sendTerminal, info else: return self.screen, reward, self.sendTerminal, info #observation, reward, terminal, info = self.env.step(action) #return self.preprocess(observation), reward, terminal, info
def __init__(self, max_random_start, observation_dims, data_format, display, use_cumulated_reward): self.max_random_start = max_random_start self.action_size = 28 self.use_cumulated_reward = use_cumulated_reward self.display = display self.data_format = data_format self.observation_dims = observation_dims self.screen = np.zeros((self.observation_dims[0],self.observation_dims[1]), np.uint8) #self.dirToTarget = 0 self.robotPose = Pose() self.goalPose = Pose() self.robotRot = 0.0 self.prevDist = 0.0 self.boom = False self.numWins = 0 self.ep_reward = 0.0 self.terminal = 0 self.sendTerminal = 0 self.readyForNewData = True rospy.wait_for_service('reset_positions') self.resetStage = rospy.ServiceProxy('reset_positions', EmptySrv) # Subscribers: # rospy.Subscriber('base_scan', LaserScan, self.scanCB,queue_size=1) # rospy.Subscriber('base_pose_ground_truth', Odometry, self.poseUpdateCB, queue_size=1) # trying time sync: sub_scan_ = message_filters.Subscriber('base_scan', LaserScan) sub_pose_ = message_filters.Subscriber('base_pose_ground_truth', Odometry) ts = message_filters.TimeSynchronizer( [sub_scan_, sub_pose_], 1) ts.registerCallback( self.syncedCB) # publishers: self.pub_vel_ = rospy.Publisher('cmd_vel', Twist, queue_size = 1) self.pub_rew_ = rospy.Publisher("lastreward",Float64,queue_size=10) self.pub_goal_ = rospy.Publisher("goal", Pose, queue_size = 15)
def step(self, action, is_training): if self.terminal == 0: if action == -1: # Step with random action action = int(random.random()*(self.action_size)) self.actionToVel( action) self.readyForNewData = True if self.display: cv2.imshow("Screen", self.screen) cv2.waitKey(3) dist = np.sqrt( (self.robotPose.position.x - self.goalPose.position.x)**2 + (self.robotPose.position.y - self.goalPose.position.y)**2) #near obstacle penalty factor: nearObstPenalty = self.minFrontDist - 1.5 reward = max( 0, ((self.prevDist - dist + 1.8)*3/dist)+min( 0, nearObstPenalty)) self.prevDist = dist if dist < 0.9: reward += 300 #Select a new goal d = -1 while d < 3.9: theta = 2.0 * np.pi * random.random() r = random.random()*19.5 self.goalPose.position.x = r*np.cos( theta) self.goalPose.position.y = r*np.sin( theta) d = np.sqrt( (self.robotPose.position.x - self.goalPose.position.x)**2 + (self.robotPose.position.y - self.goalPose.position.y)**2) self.pub_goal_.publish( self.goalPose) self.prevDist = d rwd = Float64() rwd.data = 10101.963 self.pub_rew_.publish( rwd) self.numWins += 1 if self.numWins == 99: reward += 9000 self.terminal = 1 # Add whatever info you want info = "" self.ep_reward += reward if self.terminal == 1: rewd = Float64() rewd.data = self.ep_reward self.pub_rew_.publish( rewd) self.sendTerminal = 1 while( self.readyForNewData == True): pass if self.use_cumulated_reward: return self.screen, self.ep_reward, self.sendTerminal, info else: return self.screen, reward, self.sendTerminal, info #observation, reward, terminal, info = self.env.step(action) #return self.preprocess(observation), reward, terminal, info
def __init__(self): # Give the node a name rospy.init_node('quat_to_angle', anonymous=False) # Publisher to control the robot's speed self.turtlebot_angle = rospy.Publisher('/turtlebot_angle', Float64, queue_size=5) self.turtlebot_posex = rospy.Publisher('/turtlebot_posex', Float64, queue_size=5) self.turtlebot_posey = rospy.Publisher('/turtlebot_posey', Float64, queue_size=5) #goal.target_pose.pose = Pose(Point(float(data["point"]["x"]), float(data["point"]["y"]), float(data["point"]["z"])), Quaternion(float(data["quat"]["x"]), float(data["quat"]["y"]), float(data["quat"]["z"]), float(data["quat"]["w"]))) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() while not rospy.is_shutdown(): (position, rotation) = self.get_odom() print(position) self.turtlebot_angle.publish(rotation) #print(str(position).split('x: ')[1].split('\ny:')[0]) x = float(str(position).split('x: ')[1].split('\ny:')[0]) y = float(str(position).split('y: ')[1].split('\nz:')[0]) self.turtlebot_posex.publish(x) self.turtlebot_posey.publish(y) #print(rotation) rospy.sleep(5)
