Python angle_generater 예제들

예제 #1

파일: actions_reference.py 프로젝트: DinnerHowe/cafe_robot_single

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

예제 #2

파일: target_frame.py 프로젝트: DinnerHowe/cafe_robots

 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

예제 #3

 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:

예제 #4

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')

예제 #5

파일: cruise_tasks_planner.py 프로젝트: DinnerHowe/cafe_robot_single

 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

예제 #6

파일: actions_reference.py 프로젝트: sychaichangkun/xbot_ros

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)

예제 #7

파일: actions_reference.py 프로젝트: DinnerHowe/cafe_robot_single

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)

예제 #8

파일: cruise_tasks_planner.py 프로젝트: DinnerHowe/cafe_robot_single

 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:

예제 #9

파일: actions_reference.py 프로젝트: DinnerHowe/cafe_robot_single

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