本文整理汇总了Python中sensor_msgs.msg.LaserScan方法的典型用法代码示例。如果您正苦于以下问题:Python msg.LaserScan方法的具体用法?Python msg.LaserScan怎么用?Python msg.LaserScan使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类sensor_msgs.msg
的用法示例。
在下文中一共展示了msg.LaserScan方法的11个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: __init__
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def __init__(self, ns, stack_offset):
self.__ns = ns
self.__stack_offset = stack_offset
print("stack_offset: %d"%self.__stack_offset)
self.__input_images = deque(maxlen=4 * self.__stack_offset)
#Input state
self.__input_img_pub1 = rospy.Publisher('%s/state_image1' % (self.__ns), Image, queue_size=1)
self.__input_img_pub2 = rospy.Publisher('%s/state_image2' % (self.__ns), Image, queue_size=1)
self.__input_img_pub3 = rospy.Publisher('%s/state_image3' % (self.__ns), Image, queue_size=1)
self.__input_img_pub4 = rospy.Publisher('%s/state_image4' % (self.__ns), Image, queue_size=1)
self.__occ_grid_pub = rospy.Publisher('%s/rl_map' % (self.__ns), OccupancyGrid, queue_size=1)
self.__input_scan_pub = rospy.Publisher('%s/state_scan' % (self.__ns), LaserScan, queue_size=1)
# reward info
self.__rew_pub = rospy.Publisher('%s/reward' % (self.__ns), Marker, queue_size=1)
self.__rew_num_pub = rospy.Publisher('%s/reward_num' % (self.__ns), Float64, queue_size=1)
# Waypoint info
self.__wp_pub1 = rospy.Publisher('%s/wp_vis1' % (self.__ns), PointStamped, queue_size=1)
self.__wp_pub2 = rospy.Publisher('%s/wp_vis2' % (self.__ns), PointStamped, queue_size=1)
self.__wp_pub3 = rospy.Publisher('%s/wp_vis3' % (self.__ns), PointStamped, queue_size=1)
self.__wp_pub4 = rospy.Publisher('%s/wp_vis4' % (self.__ns), PointStamped, queue_size=1)
示例2: step
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def step(self, action):
max_angular_vel = 1.5
ang_vel = ((self.action_size - 1)/2 - action) * max_angular_vel * 0.5
vel_cmd = Twist()
vel_cmd.linear.x = 0.15
vel_cmd.angular.z = ang_vel
self.pub_cmd_vel.publish(vel_cmd)
data = None
while data is None:
try:
data = rospy.wait_for_message('scan', LaserScan, timeout=5)
except:
pass
state, done = self.getState(data)
reward = self.setReward(state, done, action)
return np.asarray(state), reward, done
示例3: reset
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def reset(self):
rospy.wait_for_service('gazebo/reset_simulation')
try:
self.reset_proxy()
except (rospy.ServiceException) as e:
print("gazebo/reset_simulation service call failed")
data = None
while data is None:
try:
data = rospy.wait_for_message('scan', LaserScan, timeout=5)
except:
pass
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(data)
return np.asarray(state)
示例4: handle_turtle_pose
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def handle_turtle_pose(msg):
br = tf.TransformBroadcaster()
LaserScan laser_tf
示例5: get_observation_
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def get_observation_(self):
"""
Function returns state that will be fed to the rl-agent
It includes
the raw laser scan data,
the raw waypoint data.
:return: state
"""
waypoint = self.wp_
num_of_wps = len(waypoint.points)
state = np.ones(self.STATE_SIZE, dtype=np.float)
# add laserscan
state[0, 0:self.__scan_size, 0] = self.merged_scan_.ranges
# add goal position
wp_index = self.STATE_SIZE[1] - num_of_wps * 2
for i in range(num_of_wps):
state[0, (wp_index + i*2):(wp_index + i*2 + 2),0] = [waypoint.points[i].x, waypoint.points[i].y]
# Discretize to a resolution of 5cm.
state = np.round(np.divide(state, 0.05))*0.05
if self.debug_:
debug_scan = LaserScan()
debug_scan.header = self.merged_scan_.header
debug_scan.angle_min = self.merged_scan_.angle_min
debug_scan.angle_max = self.merged_scan_.angle_max
debug_scan.angle_increment = self.merged_scan_.angle_increment
debug_scan.range_max = 7.0
debug_scan.ranges = state[0, 0:self.__scan_size, 0]
self.debugger_.show_input_scan(debug_scan)
return state
示例6: get_observation_
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def get_observation_(self):
"""
Function returns state that will be fed to the rl-agent
It includes
the raw laser scan data,
the waypoint data in with the same format as the laser scan data.
The distance of the waypoint is saved
at the appropriate angle position in the vector.
:return: state
"""
waypoint = self.wp_
num_of_wps = len(waypoint.points)
state = np.ones(self.STATE_SIZE, dtype=np.float)
# add laserscan
state[ :, 0, 0] = self.merged_scan_.ranges
# generate wp-vector
wp_vector = np.zeros(self.STATE_SIZE[0])
for i in range(num_of_wps):
dist = math.sqrt(math.pow(waypoint.points[i].x, 2) + math.pow(waypoint.points[i].y, 2))
angle = math.atan2(waypoint.points[i].y, waypoint.points[i].x) + math.pi
wp_vector[math.floor(angle/self.merged_scan_.angle_increment)] = dist
state[:,1,0] = wp_vector
# Discretize to a resolution of 5cm.
state = np.round(np.divide(state, self.__res))*self.__res
if self.debug_:
debug_scan = LaserScan()
# debug_scan.header.frame_id = self.merged_scan_.header.frame_id
debug_scan.header = self.merged_scan_.header
debug_scan.angle_min = self.merged_scan_.angle_min
debug_scan.angle_max = self.merged_scan_.angle_max
debug_scan.angle_increment = self.merged_scan_.angle_increment
debug_scan.range_max = 7.0
debug_scan.ranges = state[:, 0, 0]
self.debugger_.show_scan_stack(debug_scan)
return state
示例7: build_laser_scan
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def build_laser_scan(self, ranges):
result = LaserScan()
result.header.stamp = rospy.Time.now()
result.angle_min = -almath.PI
result.angle_max = almath.PI
if len(ranges[1]) > 0:
result.angle_increment = (result.angle_max - result.angle_min) / len(ranges[1])
result.range_min = 0.0
result.range_max = ranges[0]
for range_it in ranges[1]:
result.ranges.append(range_it[1])
return result
# do it!
示例8: _broadcastLasers
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def _broadcastLasers(self, laser_publisher):
"""
INTERNAL METHOD, publishes the laser values in the ROS framework
Parameters:
laser_publisher - The ROS publisher for the LaserScan message,
corresponding to the laser info of the pepper robot (for API
consistency)
"""
if not self.robot.laser_manager.isActive():
return
scan = LaserScan()
scan.header.stamp = rospy.get_rostime()
scan.header.frame_id = "base_footprint"
# -120 degres, 120 degres
scan.angle_min = -2.0944
scan.angle_max = 2.0944
# 240 degres FoV, 61 points (blind zones inc)
scan.angle_increment = (2 * 2.0944) / (15.0 + 15.0 + 15.0 + 8.0 + 8.0)
# Detection ranges for the lasers in meters, 0.1 to 3.0 meters
scan.range_min = 0.1
scan.range_max = 3.0
# Fill the lasers information
right_scan = self.robot.getRightLaserValue()
front_scan = self.robot.getFrontLaserValue()
left_scan = self.robot.getLeftLaserValue()
if isinstance(right_scan, list):
scan.ranges.extend(list(reversed(right_scan)))
scan.ranges.extend([-1]*8)
if isinstance(front_scan, list):
scan.ranges.extend(list(reversed(front_scan)))
scan.ranges.extend([-1]*8)
if isinstance(left_scan, list):
scan.ranges.extend(list(reversed(left_scan)))
laser_publisher.publish(scan)
示例9: __init__
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def __init__(self, ns, train_mode):
# Class variables
self.__mode = train_mode # Mode of RL-agent (Training or Executrion ?)
self.__ns = ns # namespace of simulation environment
self.__is__new = False # True, if waypoint reached
self.__static_scan = LaserScan() # Laserscan only contains static objects
self.__ped_scan = LaserScan() # Laserscan only contains pedestrians
self.__f_scan = LaserScan()
self.__f_scan.header.frame_id = "base_footprint"
self.__b_scan = LaserScan()
self.__b_scan.header.frame_id = "base_footprint"
self.__img = OccupancyGrid() # input image
self.__wps= Waypoint() # most recent Waypoints
self.__twist = TwistStamped() # most recent velocity
self.__goal = Pose() # most recent goal position in robot frame
self.__state_mode = 2 # 0, if image as input state representation
# 1, if stacked image representation in same frame
# 2, if scan as input state representation
# Subscriber
self.wp_sub_ = rospy.Subscriber("%s/wp" % (self.__ns), Waypoint, self.__wp_callback, queue_size=1)
if ["train", "eval"].__contains__(self.__mode):
# Info only avaible during evaluation and training
self.wp_sub_reached_ = rospy.Subscriber("%s/wp_reached" % (self.__ns), Waypoint, self.__wp_reached_callback, queue_size=1)
self.static_scan_sub_ = rospy.Subscriber("%s/static_laser" % (self.__ns), LaserScan, self.__static_scan_callback,
queue_size=1)
self.ped_scan_sub_ = rospy.Subscriber("%s/ped_laser" % (self.__ns), LaserScan, self.__ped_scan_callback,
queue_size=1)
self.twist_sub_ = rospy.Subscriber("%s/twist" % (self.__ns), TwistStamped, self.__twist_callback, queue_size=1)
self.goal_sub_ = rospy.Subscriber("%s/rl_agent/robot_to_goal" % (self.__ns), PoseStamped, self.__goal_callback, queue_size=1)
else:
self.static_scan_sub_ = rospy.Subscriber("%s/b_scan" % (self.__ns), LaserScan,
self.__b_scan_callback,
queue_size=1)
self.static_scan_sub_ = rospy.Subscriber("%s/f_scan" % (self.__ns), LaserScan,
self.__f_scan_callback,
queue_size=1)
# Service
self.__img_srv = rospy.ServiceProxy('%s/image_generator/get_image' % (self.__ns), StateImageGenerationSrv)
self.__sim_in_step = rospy.ServiceProxy('%s/is_in_step' % (self.__ns), SetBool)
self.__merge_scans = rospy.ServiceProxy('%s/merge_scans' % (self.__ns), MergeScans)
示例10: __init__
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def __init__(self):
# initiliaze
rospy.init_node('GazeboWorld', anonymous=False)
#-----------Default Robot State-----------------------
self.set_self_state = ModelState()
self.set_self_state.model_name = 'mobile_base'
self.set_self_state.pose.position.x = 0.5
self.set_self_state.pose.position.y = 0.
self.set_self_state.pose.position.z = 0.
self.set_self_state.pose.orientation.x = 0.0
self.set_self_state.pose.orientation.y = 0.0
self.set_self_state.pose.orientation.z = 0.0
self.set_self_state.pose.orientation.w = 1.0
self.set_self_state.twist.linear.x = 0.
self.set_self_state.twist.linear.y = 0.
self.set_self_state.twist.linear.z = 0.
self.set_self_state.twist.angular.x = 0.
self.set_self_state.twist.angular.y = 0.
self.set_self_state.twist.angular.z = 0.
self.set_self_state.reference_frame = 'world'
#------------Params--------------------
self.depth_image_size = [160, 128]
self.rgb_image_size = [304, 228]
self.bridge = CvBridge()
self.object_state = [0, 0, 0, 0]
self.object_name = []
# 0. | left 90/s | left 45/s | right 45/s | right 90/s | acc 1/s | slow down -1/s
self.action_table = [0.4, 0.2, np.pi/6, np.pi/12, 0., -np.pi/12, -np.pi/6]
self.self_speed = [.4, 0.0]
self.default_states = None
self.start_time = time.time()
self.max_steps = 10000
self.depth_image = None
self.bump = False
#-----------Publisher and Subscriber-------------
self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size = 10)
self.set_state = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size = 10)
self.resized_depth_img = rospy.Publisher('camera/depth/image_resized',Image, queue_size = 10)
self.resized_rgb_img = rospy.Publisher('camera/rgb/image_resized',Image, queue_size = 10)
self.object_state_sub = rospy.Subscriber('gazebo/model_states', ModelStates, self.ModelStateCallBack)
self.depth_image_sub = rospy.Subscriber('camera/rgb/image_raw', Image, self.RGBImageCallBack)
self.rgb_image_sub = rospy.Subscriber('camera/depth/image_raw', Image, self.DepthImageCallBack)
self.laser_sub = rospy.Subscriber('scan', LaserScan, self.LaserScanCallBack)
self.odom_sub = rospy.Subscriber('odom', Odometry, self.OdometryCallBack)
self.bumper_sub = rospy.Subscriber('mobile_base/events/bumper', BumperEvent, self.BumperCallBack)
rospy.sleep(2.)
# What function to call when you ctrl + c
rospy.on_shutdown(self.shutdown)
示例11: _initPublishers
# 需要导入模块: from sensor_msgs import msg [as 别名]
# 或者: from sensor_msgs.msg import LaserScan [as 别名]
def _initPublishers(self):
"""
INTERNAL METHOD, initializes the ROS publishers
"""
self.front_cam_pub = rospy.Publisher(
self.ros_namespace + '/camera/front/image_raw',
Image,
queue_size=10)
self.front_info_pub = rospy.Publisher(
self.ros_namespace + '/camera/front/camera_info',
CameraInfo,
queue_size=10)
self.bottom_cam_pub = rospy.Publisher(
self.ros_namespace + '/camera/bottom/image_raw',
Image,
queue_size=10)
self.bottom_info_pub = rospy.Publisher(
self.ros_namespace + '/camera/bottom/camera_info',
CameraInfo,
queue_size=10)
self.depth_cam_pub = rospy.Publisher(
self.ros_namespace + '/camera/depth/image_raw',
Image,
queue_size=10)
self.depth_info_pub = rospy.Publisher(
self.ros_namespace + '/camera/depth/camera_info',
CameraInfo,
queue_size=10)
self.laser_pub = rospy.Publisher(
self.ros_namespace + "/laser",
LaserScan,
queue_size=10)
self.joint_states_pub = rospy.Publisher(
'/joint_states',
JointState,
queue_size=10)
self.odom_pub = rospy.Publisher(
'/naoqi_driver/odom',
Odometry,
queue_size=10)