def update(self):
        if self.distance_to_goal > self.orientation_radius:
            # Move carrot along line
            tracking_step = self.get_tracking_distance()
            c_pos = self.line.hat * tracking_step + self.current_position
            orientation = self.line.angle

            # Fill up PoseTwist
            carrot = PoseTwist(pose=Pose(
                position=tools.vector3_from_xyz_array(c_pos),
                orientation=tools.quaternion_from_rotvec([0, 0, orientation])),
                               twist=Twist(linear=Vector3(
                                   tracking_step * self.tracking_to_speed_conv,
                                   0, 0),
                                           angular=Vector3()))
            return carrot
        else:
            # Fill up PoseTwist
            carrot = PoseTwist(pose=Pose(
                position=tools.vector3_from_xyz_array(self.desired_position),
                orientation=tools.quaternion_from_rotvec(
                    self.desired_orientation)),
                               twist=Twist(linear=Vector3(),
                                           angular=Vector3()))
            return carrot
    def update(self):
        if self.distance_to_goal > self.orientation_radius:
            # Move carrot along line
            tracking_step = self.get_tracking_distance()
            c_pos = self.line.hat * tracking_step + self.current_position
            orientation = self.line.angle

            # Fill up PoseTwist
            carrot = PoseTwist(
                    pose = Pose(
                        position = tools.vector3_from_xyz_array(c_pos),
                        orientation = tools.quaternion_from_rotvec([0, 0, orientation])),

                    twist = Twist(
                        linear = Vector3(tracking_step * self.tracking_to_speed_conv, 0, 0),
                        angular = Vector3())
                    )
            return carrot
        else:
            # Fill up PoseTwist
            carrot = PoseTwist(
                    pose = Pose(
                        position = tools.vector3_from_xyz_array(self.desired_position),
                        orientation = tools.quaternion_from_rotvec(self.desired_orientation)),

                    twist = Twist(
                        linear = Vector3(),
                        angular = Vector3())
                    )
            return carrot
Exemple #3
0
    def update(self):
        '''
        # Check if in the orientation radius for the first time
        if self.redraw_line and self.distance_to_goal < self.orientation_radius:
            self.redraw_line = False
            rospy.loginfo('Redrawing trajectory line')
            self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)
        '''
        # Output a posetwist (carrot)
        distance = np.linalg.norm(self.desired_position - self.current_position)
        #angles so the boat isnt asked to straif too severly
        step_angle = {'0':self.line.angle, 
                      '1':self.line.angle+np.pi/3, 
                      '3':self.line.angle-np.pi/3,
                      '2':self.line.angle
                      }
        #desired speed in x direction
        x_velocity = {'0':0.7, 
                      '3':0, 
                      '1':0,
                      '2':0}
        
        decision = self.map_builder()
        print decision
        if decision == None:
            print 'EMPTY' 
            return PoseTwist(
                pose = Pose(
                    position = tools.vector3_from_xyz_array(self.desired_position),
                    orientation = tools.quaternion_from_rotvec(self.desired_orientation)),

                twist = Twist(
                    linear = Vector3(),        
                    angular = Vector3())
                )
        if decision in self.cell_dir_tf.keys():
            c_pos = self.look_up_step(self.cell_dir_tf[decision])
            c_pos=tools.vector3_from_xyz_array(c_pos)
            c_angle = step_angle[decision]
            c_velocity = x_velocity[decision]

        else:  
            c_pos = tools.vector3_from_xyz_array(self.current_position)
            c_angle =  self.line.angle#self.line.angle
            c_velocity = 0
        # If bproj is in threashold just set the carrot to the final position
        if distance < 0.5:
            c_pos = tools.vector3_from_xyz_array(self.current_position)
            c_angle =  self.line.angle

        carrot = PoseTwist(
                pose = Pose(
                    position = (c_pos),
                    orientation = tools.quaternion_from_rotvec([0, 0, c_angle])),
                twist = Twist(
                    linear = Vector3(c_velocity, 0, 0),        
                    angular = Vector3())
                )
        return carrot
	def get_carrot(self):
		# Project current position onto trajectory line
		Bproj = self.line.proj_pt(self.current_position)
		#rospy.loginfo('Projection: ' + str(Bproj))

		# Default carrot to desired position
		tracking_step = self.get_tracking_distance()
		c_pos = Bproj + self.overshoot * self.line.hat * tracking_step

		if self.distance_to_goal < self.orientation_radius:
			c_pos = self.desired_position

		# Fill up PoseTwistStamped
		carrot = PoseTwistStamped(
			header = Header(
				stamp = rospy.get_rostime(),
				frame_id = '/base_link'
			),
			posetwist = PoseTwist(
				pose = Pose(
					position = tools.vector3_from_xyz_array(c_pos),
					orientation = tools.quaternion_from_rotvec([0, 0, self.line.angle])),
				twist = Twist(
					linear = Vector3(tracking_step * self.tracking_to_speed_conv, 0, 0),		# Wrench Generator handles the sine of the velocity
					angular = Vector3())
				)
			)
		return carrot
Exemple #5
0
    def update(self):
        # Check if in the orientation radius for the first time
        if self.redraw_line and self.distance_to_goal < self.orientation_radius:
            self.redraw_line = False
            rospy.loginfo('Redrawing trajectory line')
            self.line = line(
                self.desired_position,
                tools.normal_vector_from_rotvec(self.desired_orientation) +
                self.desired_position)

        # Output a posetwist (carrot)
        # Project current position onto trajectory line
        Bproj = self.line.proj_pt(self.current_position)
        parallel_distance = np.linalg.norm(self.desired_position - Bproj)

        # Move carrot along line
        tracking_step = self.get_tracking_distance()
        c_pos = Bproj + self.overshoot * self.line.hat * tracking_step

        # If bproj is in threashold just set the carrot to the final position
        if parallel_distance < self.orientation_radius:
            c_pos = self.desired_position

        # Fill up PoseTwist
        carrot = PoseTwist(
            pose=Pose(position=tools.vector3_from_xyz_array(c_pos),
                      orientation=tools.quaternion_from_rotvec(
                          [0, 0, self.line.angle])),
            twist=Twist(
                linear=Vector3(
                    tracking_step * self.tracking_to_speed_conv *
                    self.overshoot, 0,
                    0),  # Wrench Generator handles the sine of the velocity
                angular=Vector3()))
        return carrot
    def get_carrot(self):
        # Project current position onto trajectory line
        Bproj = self.line.proj_pt(self.current_position)
        #rospy.loginfo('Projection: ' + str(Bproj))

        # Default carrot to desired position
        tracking_step = self.get_tracking_distance()
        c_pos = Bproj + self.overshoot * self.line.hat * tracking_step

        if self.distance_to_goal < self.orientation_radius:
            c_pos = self.desired_position

        # Fill up PoseTwistStamped
        carrot = PoseTwistStamped(
            header=Header(stamp=rospy.get_rostime(), frame_id='/base_link'),
            posetwist=PoseTwist(
                pose=Pose(position=tools.vector3_from_xyz_array(c_pos),
                          orientation=tools.quaternion_from_rotvec(
                              [0, 0, self.line.angle])),
                twist=Twist(
                    linear=Vector3(
                        tracking_step * self.tracking_to_speed_conv, 0, 0
                    ),  # Wrench Generator handles the sine of the velocity
                    angular=Vector3())))
        return carrot
Exemple #7
0
    def update(self):
        # Check if in the orientation radius for the first time
        if self.redraw_line and self.distance_to_goal < self.orientation_radius:
            self.redraw_line = False
            rospy.loginfo('Redrawing trajectory line')
            self.line = line(self.desired_position, tools.normal_vector_from_rotvec(self.desired_orientation) + self.desired_position)

        # Output a posetwist (carrot)
        # Project current position onto trajectory line
        Bproj = self.line.proj_pt(self.current_position)
        parallel_distance = np.linalg.norm(self.desired_position - Bproj)

        # Move carrot along line
        tracking_step = self.get_tracking_distance()
        c_pos = Bproj + self.overshoot * self.line.hat * tracking_step

        # If bproj is in threashold just set the carrot to the final position
        if parallel_distance < self.orientation_radius:
            c_pos = self.desired_position      

        # Fill up PoseTwist
        carrot = PoseTwist(
                pose = Pose(
                    position = tools.vector3_from_xyz_array(c_pos),
                    orientation = tools.quaternion_from_rotvec([0, 0, self.line.angle])),

                twist = Twist(
                    linear = Vector3(tracking_step * self.tracking_to_speed_conv * self.overshoot, 0, 0),        # Wrench Generator handles the sine of the velocity
                    angular = Vector3())
                )
        return carrot
Exemple #8
0
 def update(self):
     # Fill up PoseTwist
     carrot = PoseTwist(pose=Pose(position=tools.vector3_from_xyz_array(
         self.desired_position),
                                  orientation=tools.quaternion_from_rotvec(
                                      self.desired_orientation)),
                        twist=Twist(linear=Vector3(), angular=Vector3()))
     return carrot
Exemple #9
0
    def get_current_posetwist(self):
        return PoseTwist(
                pose = Pose(
                    position = tools.vector3_from_xyz_array(self.current_position),
                    orientation = tools.quaternion_from_rotvec(self.current_orientation)),

                twist = Twist(
                    linear = Vector3(),
                    angular = Vector3())
                )
Exemple #10
0
    def update(self):
        # Fill up PoseTwist
        carrot = PoseTwist(
                pose = Pose(
                    position = tools.vector3_from_xyz_array(self.desired_position),
                    orientation = tools.quaternion_from_rotvec(self.desired_orientation)),

                twist = Twist(
                    linear = Vector3(),
                    angular = Vector3())
                )
        return carrot
Exemple #11
0
    def update(self, event):
        # Lock on odom cb
        self.as_lock.acquire()
        #print 'update lock'

        # Make a header
        header = Header(
                frame_id = '/enu',
                stamp = rospy.get_rostime())

        # Get a posetwist
        posetwist = PoseTwist()
        if self.traj_gen is not None:
            posetwist = self.traj_gen.update()

        # Publish trajectory
        traj = PoseTwistStamped(
            header = header,
            posetwist = posetwist
            )

        # Publish desired pose for RVIZ
        self.traj_debug_pub.publish(PoseStamped(header=header, pose=traj.posetwist.pose))
        self.goal_debug_pub.publish(PoseStamped(header=header, pose=
            Pose(
                position = tools.vector3_from_xyz_array(self.desired_position),
                orientation = tools.quaternion_from_rotvec(self.desired_orientation))))

        # Killed logic
        if not self.killed:
            self.traj_pub.publish(traj)

        rospy.logdebug('Angle to goal: ' + str(self.angle_to_goal_orientation[2] * 180 / np.pi) + '\t\t\tDistance to goal: ' + str(self.distance_to_goal))

        # Check if goal is reached
        if self.moveto_as.is_active():
            if self.distance_to_goal < self.linear_tolerance:
                if abs(self.angle_to_goal_orientation[2]) < self.angular_tolerance:
                    rospy.loginfo('Reached goal')
                    self.moveto_as.set_succeeded(None)

        # Release lock
        self.as_lock.release()