def odom_callback(self,data): self.odom_pose=data.pose.pose.position broadcaster = tf.TransformBroadcaster() angle=move_reference.angle_generater(self.msg.point,self.odom_pose) quaterion=move_reference.angle_to_quat(angle) broadcaster.sendTransform((self.msg.point.x,self.msg.point.y,self.msg.point.z),quaterion,rospy.Time.now(),'target_frame','map') print self.msg
def MultiTasksPlanCB(self, path):
path_poses = path.poses
path_num = len(path_poses)
Sessor = 0.5
if path_num > 100:
rospy.loginfo('long path model' + '%s' % len(path_poses))
self.move_base.cancel_goal()
rospy.loginfo('cancel old goal and create a new goal')
new_goal = MoveBaseGoal()
new_point = path_poses[int(path_num * Sessor)].pose.position
new_angle = move_reference.angle_generater(
new_point,
path_poses[int(path_num * Sessor) + 1].pose.position)
new_goal.target_pose.header.frame_id = 'map'
new_goal.target_pose.pose.position = new_point
(new_goal.target_pose.pose.orientation.x,
new_goal.target_pose.pose.orientation.y,
new_goal.target_pose.pose.orientation.z,
new_goal.target_pose.pose.orientation.w
) = move_reference.angle_to_quat(new_angle)
new_goal.target_pose.header.seq = self.seq_num
new_goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(new_goal)
self.seq_num += 1
"""else:
def multitasks(intial_point, point):
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
goal = MoveBaseGoal()
init_point = intial_point
angle = move_reference.angle_generater(point, init_point)
try:
goal.target_pose.header.frame_id = pose.header.frame_id
except:
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position = point
(goal.target_pose.pose.orientation.x, goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w) = move_reference.angle_to_quat(angle)
move_base.send_goal(goal)
finished_within_time = move_base.wait_for_result(rospy.Duration(300))
if not finished_within_time:
move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo('fail to acheive goal')
def path_callback(path, point):
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
path_poses = path.poses
path_num = len(path_poses)
#print 'path_distance: ',path_num
if path_num > 100:
print 'long path model'
move_base.cancel_goal()
print 'cancel old goal and create a new goal'
new_goal = MoveBaseGoal()
new_angle = move_reference.angle_generater(
path_poses[int(path_num * 0.8)].pose.position,
path_poses[int(path_num * 0.7)].pose.position)
try:
new_goal.target_pose.header.frame_id = pose.header.frame_id
except:
new_goal.target_pose.header.frame_id = 'map'
new_goal.target_pose.pose.position = point
(new_goal.target_pose.pose.orientation.x,
new_goal.target_pose.pose.orientation.y,
new_goal.target_pose.pose.orientation.z,
new_goal.target_pose.pose.orientation.w
) = move_reference.angle_to_quat(new_angle)
move_base.send_goal(new_goal)
else:
pass
def SendGoal(self):
self.goal.target_pose.pose.position = self.Plist[self.count]
self.goal.target_pose.header.seq = self.seq_num
self.goal.target_pose.header.stamp = rospy.Time.now()
angle = move_reference.angle_generater(self.Plist[self.count],
self.Current_position.position)
(self.goal.target_pose.pose.orientation.x,
self.goal.target_pose.pose.orientation.y,
self.goal.target_pose.pose.orientation.z,
self.goal.target_pose.pose.orientation.w
) = move_reference.angle_to_quat(angle)
#print 'send current goal: ', self.count , '\n', self.goal
self.move_base.send_goal(self.goal)
self.seq_num += 1
def tasks(intial_point, point):
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
goal = MoveBaseGoal()
init_point = intial_point
angle = move_reference.angle_generater(point, init_point)
try:
goal.target_pose.header.frame_id = pose.header.frame_id
except:
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position = point
(goal.target_pose.pose.orientation.x, goal.target_pose.pose.orientation.y,
goal.target_pose.pose.orientation.z,
goal.target_pose.pose.orientation.w) = move_reference.angle_to_quat(angle)
move_base.send_goal(goal)
def MultiTasksPlanCB(self, path):
path_poses = path.poses
path_num = len(path_poses)
Sessor = 0.5
if path_num > 100:
rospy.loginfo('long path model' + '%s'%len(path_poses))
self.move_base.cancel_goal()
rospy.loginfo('cancel old goal and create a new goal')
new_goal = MoveBaseGoal()
new_point = path_poses[int(path_num * Sessor)].pose.position
new_angle = move_reference.angle_generater(new_point, path_poses[int(path_num * Sessor) + 1].pose.position)
new_goal.target_pose.header.frame_id = 'map'
new_goal.target_pose.pose.position = new_point
(new_goal.target_pose.pose.orientation.x, new_goal.target_pose.pose.orientation.y, new_goal.target_pose.pose.orientation.z, new_goal.target_pose.pose.orientation.w) = move_reference.angle_to_quat(new_angle)
new_goal.target_pose.header.seq = self.seq_num
new_goal.target_pose.header.stamp = rospy.Time.now()
self.move_base.send_goal(new_goal)
self.seq_num += 1
"""else:
def SendGoal(self): self.goal.target_pose.pose.position = self.Plist[self.count] self.goal.target_pose.header.seq = self.seq_num self.goal.target_pose.header.stamp = rospy.Time.now() angle = move_reference.angle_generater(self.Plist[self.count], self.Current_position.position) (self.goal.target_pose.pose.orientation.x, self.goal.target_pose.pose.orientation.y, self.goal.target_pose.pose.orientation.z, self.goal.target_pose.pose.orientation.w) = move_reference.angle_to_quat(angle) #print 'send current goal: ', self.count , '\n', self.goal self.move_base.send_goal(self.goal) self.seq_num += 1
def path_callback(path,point):
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
path_poses=path.poses
path_num=len(path_poses)
#print 'path_distance: ',path_num
if path_num>100:
print 'long path model'
move_base.cancel_goal()
print 'cancel old goal and create a new goal'
new_goal = MoveBaseGoal()
new_angle=move_reference.angle_generater(path_poses[int(path_num*0.8)].pose.position,path_poses[int(path_num*0.7)].pose.position)
try:
new_goal.target_pose.header.frame_id = pose.header.frame_id
except:
new_goal.target_pose.header.frame_id = 'map'
new_goal.target_pose.pose.position=point
(new_goal.target_pose.pose.orientation.x, new_goal.target_pose.pose.orientation.y, new_goal.target_pose.pose.orientation.z, new_goal.target_pose.pose.orientation.w) = move_reference.angle_to_quat(new_angle)
move_base.send_goal(new_goal)
else:
pass
def tasks(intial_point,point):
move_base = actionlib.SimpleActionClient('move_base', MoveBaseAction)
move_base.wait_for_server()
goal = MoveBaseGoal()
init_point=intial_point
angle=move_reference.angle_generater(point,init_point)
try:
goal.target_pose.header.frame_id = pose.header.frame_id
except:
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position=point
(goal.target_pose.pose.orientation.x,goal.target_pose.pose.orientation.y,goal.target_pose.pose.orientation.z,goal.target_pose.pose.orientation.w)=move_reference.angle_to_quat(angle)
move_base.send_goal(goal)
