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