本文整理汇总了Python中tf.TransformListener.canTransform方法的典型用法代码示例。如果您正苦于以下问题:Python TransformListener.canTransform方法的具体用法?Python TransformListener.canTransform怎么用?Python TransformListener.canTransform使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类tf.TransformListener
的用法示例。
在下文中一共展示了TransformListener.canTransform方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: main
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
def main():
rospy.init_node('tf_test')
rospy.loginfo("Node for testing actionlib server")
#from_link = '/head_color_camera_l_link'
#to_link = '/base_link'
from_link = '/base_link'
to_link = '/map'
tfl = TransformListener()
rospy.sleep(5)
t = rospy.Time(0)
mpose = PoseStamped()
mpose.pose.position.x = 1
mpose.pose.position.y = 0
mpose.pose.position.z = 0
mpose.pose.orientation.x = 0
mpose.pose.orientation.y = 0
mpose.pose.orientation.z = 0
mpose.pose.orientation.w = 0
mpose.header.frame_id = from_link
mpose.header.stamp = rospy.Time.now()
rospy.sleep(5)
mpose_transf = None
rospy.loginfo('Waiting for transform for some time...')
tfl.waitForTransform(to_link,from_link,t,rospy.Duration(5))
if tfl.canTransform(to_link,from_link,t):
mpose_transf = tfl.transformPose(to_link,mpose)
print mpose_transf
else:
rospy.logerr('Transformation is not possible!')
sys.exit(0)
示例2: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
class Demo:
def __init__(self, goals):
rospy.init_node("demo", anonymous=True)
self.frame = rospy.get_param("~frame")
self.pubGoal = rospy.Publisher("goal", PoseStamped, queue_size=1)
self.listener = TransformListener()
self.goals = goals
self.goalIndex = 0
def run(self):
self.listener.waitForTransform("/world", self.frame, rospy.Time(), rospy.Duration(5.0))
goal = PoseStamped()
goal.header.seq = 0
goal.header.frame_id = "world"
while not rospy.is_shutdown():
goal.header.seq += 1
goal.header.stamp = rospy.Time.now()
goal.pose.position.x = self.goals[self.goalIndex][0]
goal.pose.position.y = self.goals[self.goalIndex][1]
goal.pose.position.z = self.goals[self.goalIndex][2]
quaternion = tf.transformations.quaternion_from_euler(0, 0, self.goals[self.goalIndex][3])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.pubGoal.publish(goal)
t = self.listener.getLatestCommonTime("/world", self.frame)
if self.listener.canTransform("/world", self.frame, t):
position, quaternion = self.listener.lookupTransform("/world", self.frame, t)
rpy = tf.transformations.euler_from_quaternion(quaternion)
if (
math.fabs(position[0] - self.goals[self.goalIndex][0]) < 0.3
and math.fabs(position[1] - self.goals[self.goalIndex][1]) < 0.3
and math.fabs(position[2] - self.goals[self.goalIndex][2]) < 0.3
and math.fabs(rpy[2] - self.goals[self.goalIndex][3]) < math.radians(10)
and self.goalIndex < len(self.goals) - 1
):
rospy.sleep(self.goals[self.goalIndex][4])
self.goalIndex += 1
示例3: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
# TODO create particles
self.particle_cloud = self.create_initial_particle_list(xy_theta)
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
w_sum = sum([p.w for p in self.particle_cloud])
for particle in self.particle_cloud:
particle.w /= w_sum
# TODO: implement this
def publish_particles(self, msg):
print('publish_particles')
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
print('scan_received')
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=rospy.Time(0),frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
print('broadcast')
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.Time.now(),
self.odom_frame,
self.map_frame)
示例4: Collector
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
class Collector():
def __init__(self):
rospy.init_node('collector', anonymous = False)
self.lis=TransformListener();
self.data_out=SBC_Output();
rospy.Subscriber("/joint_states", JointState, self.j_callback)
rospy.Subscriber("/finger1/ContactState", KCL_ContactStateStamped, self.f_callback1)
rospy.Subscriber("/finger2/ContactState", KCL_ContactStateStamped, self.f_callback2)
rospy.Subscriber("/pressure", PressureControl, self.p_callback)
rospy.Subscriber("/prob_fail", Float64, self.prob_callback)
self.publisher=rospy.Publisher("sbc_data", SBC_Output)
self.point1=PointStamped()
self.point2=PointStamped()
self.rate=rospy.Rate(20);
def getParams(self):
self.data_out.D_Gain=rospy.get_param("/bhand_pid/d_gain")
self.data_out.F_ref_pid=rospy.get_param("/bhand_pid/f_ref")
self.data_out.I_Gain=rospy.get_param("/bhand_pid/i_gain")
self.data_out.P_Gain=rospy.get_param("/bhand_pid/p_gain")
self.data_out.freq=rospy.get_param("/pressure_reg/frequency")
self.data_out.Beta=rospy.get_param("/bhand_sbc/beta")
self.data_out.Delta=rospy.get_param("/bhand_sbc/delta")
self.data_out.Eta=rospy.get_param("/bhand_sbc/eta")
self.data_out.F_ref_sbc=rospy.get_param("/bhand_sbc/f_ref")
def j_callback(self,data):
self.joints=data;
self.data_out.effort1=data.effort[1]
self.data_out.effort2=data.effort[0]
def f_callback1(self,data):
self.data_out.Fn1=data.Fnormal;
ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z])
self.data_out.Ft1=np.sqrt(ft.dot(ft));
self.point1=PointStamped();
self.point1.header=data.header;
self.point1.point=data.contact_position;
def f_callback2(self,data):
self.data_out.Fn2=data.Fnormal;
ft=np.array([data.tangential_force.x,data.tangential_force.y,data.tangential_force.z])
self.data_out.Ft2=np.sqrt(ft.dot(ft));
self.point2=PointStamped();
self.point2.header=data.header;
self.point2.point=data.contact_position;
def p_callback(self,data):
self.data_out.p_demand=data.p_demand;
self.data_out.p_measure=data.p_measure;
def prob_callback(self,data):
self.data_out.Pfailure=data.data;
def transform_it(self,data):
if(data.header.frame_id):
#data.header.stamp=rospy.Time.now();
if(self.lis.canTransform("base_link",data.header.frame_id,data.header.stamp) or True):
#print(rospy.Time.now())
data.header.stamp=data.header.stamp-rospy.Duration(0.02)
#point=self.lis.transformPoint("base_link", data)
try:
#self.lis.waitForTransform("base_link", data.header.frame_id, data.header.stamp, rospy.Duration(1))
# print(rospy.Time.now())
self.point=self.lis.transformPoint("base_link", data)
return True
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF problem 2")
pass
else:
rospy.logwarn("Cannot Transform")
else:
print(data.header.frame_id)
return False
def get_distance(self,point1,point2):
d=np.array([point1.x-point2.x, point1.y-point2.y, point1.z-point2.z])
return np.sqrt(d.dot(d));
def send_it(self):
while not rospy.is_shutdown():
self.data_out.header.stamp=rospy.Time.now();
self.getParams()
got_it=self.transform_it(self.point1);
if(got_it):
self.data_out.contact1=self.point.point
got_it=self.transform_it(self.point2);
if(got_it):
self.data_out.contact2=self.point.point
self.data_out.distance=self.get_distance(self.data_out.contact1,self.data_out.contact2)*100
self.publisher.publish(self.data_out);
self.rate.sleep();
示例5: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
@staticmethod
def weighted_values(values, probabilities, size):
""" Return a random sample of size elements form the set values with the specified probabilities
values: the values to sample from (numpy.ndarray)
probabilities: the probability of selecting each element in values (numpy.ndarray)
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
# TODO create particles
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
# TODO: implement this
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data. Feel free to modify this, however,
I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.resample_particles() # resample particles to focus on areas of high density
self.update_robot_pose() # update robot's pose
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
(translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map to odom transformation.
This is necessary so things like move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
示例6: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
size: the number of samples
"""
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
def update_initial_pose(self, msg):
""" Callback function to handle re-initializing the particle filter based on a pose estimate.
These pose estimates could be generated by another ROS Node or could come from the rviz GUI """
xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(msg.pose.pose)
self.initialize_particle_cloud(xy_theta)
self.fix_map_to_odom_transform(msg)
def initialize_particle_cloud(self, xy_theta=None):
""" Initialize the particle cloud.
Arguments
xy_theta: a triple consisting of the mean x, y, and theta (yaw) to initialize the
particle cloud around. If this input is ommitted, the odometry will be used """
if xy_theta == None:
xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
for i in range(self.n_particles):
self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,.25),y=xy_theta[1]+gauss(0,.25),theta=xy_theta[2]+gauss(0,.25)))
self.normalize_particles()
self.update_robot_pose()
""" Level 1 """
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
z = 0.0
for p in self.particle_cloud:
z += p.w
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w /= z
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id=self.map_frame),poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data. Feel free to modify this, however,
I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id=self.base_frame), pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.resample_particles() # resample particles to focus on areas of high density
self.update_robot_pose() # update robot's pose
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" Super tricky code to properly update map to odom transform... do not modify this... Difficulty level infinity. """
(translation, rotation) = TransformHelpers.convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=TransformHelpers.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = TransformHelpers.convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map to odom transformation.
This is necessary so things like move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation, self.rotation, rospy.get_rostime(), self.odom_frame, self.map_frame)
示例7: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
y = random_sample()* map_info.height * map_info.resolution * 0.1
if random_sample() > 0.5:
y = -y
theta = random_sample() * math.pi*2
self.particle_cloud.append(Particle(x, y, theta))
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e.
sum to 1.0) """
sum = 0
for particle in self.particle_cloud:
sum += particle.w
for particle in self.particle_cloud:
particle.w /= sum
def publish_particles(self, pub):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
pub.publish(
PoseArray(header=Header(stamp=rospy.Time.now(), frame_id=self.map_frame), poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data. Feel free to modify this, however,
I hope it will provide a good guide. The input msg is an object of type sensor_msgs/LaserScan """
if not self.initialized:
# wait for initialization to complete
return
if not (self.tf_listener.canTransform(self.base_frame, msg.header.frame_id, rospy.Time(0))):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
rospy.logwarn("can't transform to laser scan")
return
if not (self.tf_listener.canTransform(self.base_frame, self.odom_frame, rospy.Time(0))):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
rospy.logwarn("can't transform to base frame")
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header = Header(stamp = rospy.Time(0),
frame_id = msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame, p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header = Header(stamp = rospy.Time(0),
frame_id = self.base_frame))
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = TransformHelpers.convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not self.particle_cloud:
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
示例8: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
#Soma total das probabilidades das particulas
w_sum = sum([p.w for p in self.particle_cloud])
#Dividir cada probabilidade de uma particula pela soma total
for particle in self.particle_cloud:
particle.w /= w_sum
# TODO: implement this
def publish_particles(self, msg):
print("Publicar Particulas.")
print(len(self.particle_cloud))
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
print("Not Initialized")
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,rospy.Time(0))):
print("Not 2")
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,rospy.Time(0))):
print("Not 3")
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp = rospy.Time(0),
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
print("PASSOU")
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
示例9: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
for i in range(self.n_particles):
self.particle_cloud.append(Particle(x=xy_theta[0]+gauss(0,0.25),y=xy_theta[1]+gauss(0,0.25),theta=xy_theta[2]+gauss(0,0.25)))
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
w = [deepcopy(p.w) for p in self.particle_cloud]
z = sum(w)
print(z)
if z > 0:
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w = w[i] / z
else:
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w = 1/len(self.particle_cloud)
print(sum([p.w for p in self.particle_cloud]))
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
示例10: Controller
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
#Button 5
if delta[4] == 1:
self.square_go()
#self.targetX = square[0][0]
#self.targetY = square[0][1]
#self.targetZ = square[0][2]
#self.des_angle = square[0][3]
#print(self.targetZ)
#self.power += 100.0
#print(self.power)
self.state = Controller.Automatic
#Button 6
if delta[5] == 1:
self.square_start = False
self.targetX = 0.0
self.targetY = 0.0
self.targetZ = 0.5
self.des_angle = 0.0
#print(self.targetZ)
#self.power -= 100.0
#print(self.power)
self.state = Controller.Automatic
self.lastJoy = data
def run(self):
thrust = 0
print("jello")
while not rospy.is_shutdown():
if self.state == Controller.TakeOff:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
print(t,self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t))
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
print(position[0],position[1],position[2])
#if position[2] > 2.0 or thrust > 54000:
if thrust > 55000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
#self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 500
#self.power = thrust
msg = self.cmd_vel_telop
msg.linear.z = thrust
self.pubNav.publish(msg)
if self.state == Controller.Land:
t = self.listener.getLatestCommonTime("/mocap", "/Nano_Mark_Gon4")
if self.listener.canTransform("/mocap", "/Nano_Mark_Gon4", t):
position, quaternion = self.listener.lookupTransform("/mocap","/Nano_Mark_Gon4", t)
if position[2] > 0.05:
msg_land = self.cmd_vel_telop
self.power -=100
msg_land.linear.z = self.power
self.pubNav.publish(msg_land)
else:
msg_land = self.cmd_vel_telop
msg_land.linear.z = 0
self.pubNav.publish(msg_land)
示例11: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
tot_weight = sum([particle.w for particle in self.particle_cloud]) or 1
for particle in self.particle_cloud:
particle.w = particle.w/tot_weight;
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
marker_array = []
for index, particle in enumerate(self.particle_cloud):
marker = Marker(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
pose=particle.as_pose(),
type=0,
scale=Vector3(x=particle.w*2,y=particle.w*1,z=particle.w*5),
id=index,
color=ColorRGBA(r=1,a=1))
marker_array.append(marker)
self.marker_pub.publish(MarkerArray(markers=marker_array))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
示例12: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
class Controller:
Idle = 0
Automatic = 1
TakingOff = 2
Landing = 3
def __init__(self, frame):
self.frame = frame
self.pubNav = rospy.Publisher('cmd_vel', Twist, queue_size=1)
self.listener = TransformListener()
self.pidX = PID(35, 10, 0.0, -20, 20, "x")
self.pidY = PID(-35, -10, -0.0, -20, 20, "y")
self.pidZ = PID(4000, 3000.0, 2000.0, 10000, 60000, "z")
self.pidYaw = PID(-50.0, 0.0, 0.0, -200.0, 200.0, "yaw")
self.state = Controller.Idle
self.goal = Pose()
rospy.Subscriber("goal", Pose, self._poseChanged)
rospy.Service("takeoff", std_srvs.srv.Empty, self._takeoff)
rospy.Service("land", std_srvs.srv.Empty, self._land)
def getTransform(self, source_frame, target_frame):
now = rospy.Time.now()
success = False
if self.listener.canTransform(source_frame, target_frame, rospy.Time(0)):
t = self.listener.getLatestCommonTime(source_frame, target_frame)
if self.listener.canTransform(source_frame, target_frame, t):
position, quaternion = self.listener.lookupTransform(source_frame, target_frame, t)
success = True
delta = (now - t).to_sec() * 1000 #ms
if delta > 25:
rospy.logwarn("Latency: %f ms.", delta)
# self.listener.clear()
# rospy.sleep(0.02)
if success:
return position, quaternion, t
def pidReset(self):
self.pidX.reset()
self.pidZ.reset()
self.pidZ.reset()
self.pidYaw.reset()
def _poseChanged(self, data):
self.goal = data
def _takeoff(self, req):
rospy.loginfo("Takeoff requested!")
self.state = Controller.TakingOff
return std_srvs.srv.EmptyResponse()
def _land(self, req):
rospy.loginfo("Landing requested!")
self.state = Controller.Landing
return std_srvs.srv.EmptyResponse()
def run(self):
thrust = 0
while not rospy.is_shutdown():
now = rospy.Time.now()
if self.state == Controller.TakingOff:
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
if position[2] > 0.05 or thrust > 50000:
self.pidReset()
self.pidZ.integral = thrust / self.pidZ.ki
self.targetZ = 0.5
self.state = Controller.Automatic
thrust = 0
else:
thrust += 100
msg = Twist()
msg.linear.z = thrust
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Landing:
self.goal.position.z = 0.05
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
if position[2] <= 0.1:
self.state = Controller.Idle
msg = Twist()
self.pubNav.publish(msg)
else:
rospy.logerr("Could not transform from /world to %s.", self.frame)
if self.state == Controller.Automatic or self.state == Controller.Landing:
# transform target world coordinates into local coordinates
r = self.getTransform("/world", self.frame)
if r:
position, quaternion, t = r
targetWorld = PoseStamped()
targetWorld.header.stamp = t
targetWorld.header.frame_id = "world"
targetWorld.pose = self.goal
#.........这里部分代码省略.........
示例13: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
for i in range(self.n_particles):
self.particle_cloud.append(Particle(randn(), randn(),randn())) #add robot location guess to each number
self.normalize_particles()
self.update_robot_pose()
#print self.particle_cloud
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
pass
# TODO: implement this
#add up all weights of all particles, divide each particle by this number
sumWeight = 0
for particle in self.particle_cloud:
sumWeight += particle.w
for particle in self.particle_cloud:
particle.w /=sumWeight
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
示例14: frame
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
if xy_theta == None:
xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
self.particle_cloud = []
self.particle_cloud.append(Particle(xy_theta[0],xy_theta[1],xy_theta[2]))
# Initialize particle cloud with a decent amount of noise
for i in range (0,self.number_of_particles):
self.particle_cloud.append(Particle(xy_theta[0]+np.random.randn()*.5,xy_theta[1]+np.random.randn()*.5,xy_theta[2]+np.random.randn()*.5))
self.normalize_particles()
self.update_robot_pose()
def normalize_particles(self):
""" Make sure the particle weights define a valid distribution (i.e. sum to 1.0) """
particle_sum = 0
# Sum up particle weights to divide by for normalization
for particle in self.particle_cloud:
particle_sum += particle.w
# Make all particle weights add to 1
for particle in self.particle_cloud:
particle.w /= particle_sum
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),
frame_id=self.map_frame),
poses=particles_conv))
def scan_received(self, msg):
""" This is the default logic for what to do when processing scan data.
Feel free to modify this, however, I hope it will provide a good
guide. The input msg is an object of type sensor_msgs/LaserScan """
if not(self.initialized):
# wait for initialization to complete
return
if not(self.tf_listener.canTransform(self.base_frame,msg.header.frame_id,msg.header.stamp)):
# need to know how to transform the laser to the base frame
# this will be given by either Gazebo or neato_node
return
if not(self.tf_listener.canTransform(self.base_frame,self.odom_frame,msg.header.stamp)):
# need to know how to transform between base and odometric frames
# this will eventually be published by either Gazebo or neato_node
return
# calculate pose of laser relative ot the robot base
p = PoseStamped(header=Header(stamp=rospy.Time(0),
frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose(self.base_frame,p)
# find out where the robot thinks it is based on its odometry
p = PoseStamped(header=Header(stamp=msg.header.stamp,
frame_id=self.base_frame),
pose=Pose())
self.odom_pose = self.tf_listener.transformPose(self.odom_frame, p)
# store the the odometry pose in a more convenient format (x,y,theta)
new_odom_xy_theta = convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud):
# now that we have all of the necessary transforms we can update the particle cloud
self.initialize_particle_cloud()
# cache the last odometric pose so we can only update our particle filter if we move more than self.d_thresh or self.a_thresh
self.current_odom_xy_theta = new_odom_xy_theta
# update our map to odom transform now that the particles are initialized
self.fix_map_to_odom_transform(msg)
elif (math.fabs(new_odom_xy_theta[0] - self.current_odom_xy_theta[0]) > self.d_thresh or
math.fabs(new_odom_xy_theta[1] - self.current_odom_xy_theta[1]) > self.d_thresh or
math.fabs(new_odom_xy_theta[2] - self.current_odom_xy_theta[2]) > self.a_thresh):
# we have moved far enough to do an update!
self.update_particles_with_odom(msg) # update based on odometry
self.update_particles_with_laser(msg) # update based on laser scan
self.update_robot_pose() # update robot's pose
self.resample_particles() # resample particles to focus on areas of high density
self.fix_map_to_odom_transform(msg) # update map to odom transform now that we have new particles
# publish particles (so things like rviz can see them)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg):
""" This method constantly updates the offset of the map and
odometry coordinate systems based on the latest results from
the localizer """
(translation, rotation) = convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=convert_translation_rotation_to_pose(translation,rotation),
header=Header(stamp=msg.header.stamp,frame_id=self.base_frame))
self.odom_to_map = self.tf_listener.transformPose(self.odom_frame, p)
(self.translation, self.rotation) = convert_pose_inverse_transform(self.odom_to_map.pose)
def broadcast_last_transform(self):
""" Make sure that we are always broadcasting the last map
to odom transformation. This is necessary so things like
move_base can work properly. """
if not(hasattr(self,'translation') and hasattr(self,'rotation')):
return
self.tf_broadcaster.sendTransform(self.translation,
self.rotation,
rospy.get_rostime(),
self.odom_frame,
self.map_frame)
示例15: __init__
# 需要导入模块: from tf import TransformListener [as 别名]
# 或者: from tf.TransformListener import canTransform [as 别名]
#.........这里部分代码省略.........
return d2
@staticmethod
def weighted_values(values, probabilities, size):
bins = np.add.accumulate(probabilities)
return values[np.digitize(random_sample(size), bins)]
@staticmethod
def convert_pose_to_xy_and_theta(pose):
orientation_tuple = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
angles = euler_from_quaternion(orientation_tuple)
return (pose.position.x, pose.position.y, angles[2])
def initialize_particle_cloud(self):
self.particle_cloud_initialized = True
(x,y,theta) = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose)
for i in range(self.n_particles):
self.particle_cloud.append(Particle(x=x+gauss(0,.25),y=y+gauss(0,.25),theta=theta+gauss(0,.25)))
self.normalize_particles()
def normalize_particles(self):
z = 0.0
for p in self.particle_cloud:
z += p.w
for i in range(len(self.particle_cloud)):
self.particle_cloud[i].w /= z
def publish_particles(self, msg):
particles_conv = []
for p in self.particle_cloud:
particles_conv.append(p.as_pose())
# actually send the message so that we can view it in rviz
self.particle_pub.publish(PoseArray(header=Header(stamp=rospy.Time.now(),frame_id="map"),poses=particles_conv))
def scan_received(self, msg):
if not(self.initialized):
return
if not(self.tf_listener.canTransform("base_footprint",msg.header.frame_id,msg.header.stamp)):
return
if not(self.tf_listener.canTransform("base_footprint","odom",msg.header.stamp)):
return
p = PoseStamped(header=Header(stamp=rospy.Time(0),frame_id=msg.header.frame_id))
self.laser_pose = self.tf_listener.transformPose("base_footprint",p)
p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose())
#p = PoseStamped(header=Header(stamp=msg.header.stamp,frame_id="base_footprint"), pose=Pose(position=Point(x=0.0,y=0.0,z=0.0),orientation=Quaternion(x=0.0,y=0.0,z=0.0,w=0.0)))
self.odom_pose = self.tf_listener.transformPose("odom", p)
new_odom_xy_theta = MyAMCL.convert_pose_to_xy_and_theta(self.odom_pose.pose)
if not(self.particle_cloud_initialized):
self.initialize_particle_cloud()
self.update_robot_pose()
self.current_odom_xy_theta = new_odom_xy_theta
self.fix_map_to_odom_transform(msg)
else:
delta = (new_odom_xy_theta[0] - self.current_odom_xy_theta[0], new_odom_xy_theta[1] - self.current_odom_xy_theta[1], new_odom_xy_theta[2] - self.current_odom_xy_theta[2])
if math.fabs(delta[0]) > self.d_thresh or math.fabs(delta[1]) > self.d_thresh or math.fabs(delta[2]) > self.a_thresh:
self.update_particles_with_odom(msg)
self.update_robot_pose()
self.update_particles_with_laser(msg)
self.resample_particles()
self.update_robot_pose()
self.fix_map_to_odom_transform(msg)
else:
self.fix_map_to_odom_transform(msg, False)
self.publish_particles(msg)
def fix_map_to_odom_transform(self, msg, recompute_odom_to_map=True):
if recompute_odom_to_map:
(translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.robot_pose)
p = PoseStamped(pose=MyAMCL.convert_translation_rotation_to_pose(translation,rotation),header=Header(stamp=msg.header.stamp,frame_id="base_footprint"))
self.odom_to_map = self.tf_listener.transformPose("odom", p)
(translation, rotation) = MyAMCL.convert_pose_inverse_transform(self.odom_to_map.pose)
self.tf_broadcaster.sendTransform(translation, rotation, msg.header.stamp+rospy.Duration(1.0), "odom", "map")
@staticmethod
def convert_translation_rotation_to_pose(translation,rotation):
return Pose(position=Point(x=translation[0],y=translation[1],z=translation[2]), orientation=Quaternion(x=rotation[0],y=rotation[1],z=rotation[2],w=rotation[3]))
@staticmethod
def convert_pose_inverse_transform(pose):
translation = np.zeros((4,1))
translation[0] = -pose.position.x
translation[1] = -pose.position.y
translation[2] = -pose.position.z
translation[3] = 1.0
rotation = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)
euler_angle = euler_from_quaternion(rotation)
rotation = np.transpose(rotation_matrix(euler_angle[2], [0,0,1])) # the angle is a yaw
transformed_translation = rotation.dot(translation)
translation = (transformed_translation[0], transformed_translation[1], transformed_translation[2])
rotation = quaternion_from_matrix(rotation)
return (translation, rotation)