def __init__(self):
# First set up service
request_trajectory_service = rospy.ServiceProxy(
"generate_toppra_trajectory", GenerateTrajectory)
trajectory_pub = rospy.Publisher('trajectory',
MultiDOFJointTrajectory, queue_size=1)
time.sleep(2.5)
# Example of a simple square trajectory for quadcopter. All vectors
# must be of same length
x = [-0.5, 0.5, 0.5, -0.5, -0.5]
y = [-0.5, -0.5, 0.5, 0.5, -0.5]
z = [0, 0, 0, 0, 0]
yaw = [0, 0, 0, 0, 0]
# Create a service request which will be filled with waypoints
request = GenerateTrajectoryRequest()
waypoint = JointTrajectoryPoint()
for i in range(0, len(x)):
waypoint.positions = [x[i], y[i], z[i], yaw[i]]
waypoint.velocities = [0.7, 0.7, 1, 1]
waypoint.accelerations = [0.08, 0.08, 1, 1]
request.waypoints.points.append(copy.deepcopy(waypoint))
request.waypoints.joint_names = ["x", "y", "z", "yaw"]
request.sampling_frequency = 100.0
response = request_trajectory_service(request)
print "Converting trajectory to multi dof"
joint_trajectory = response.trajectory
multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory(joint_trajectory)
trajectory_pub.publish(multi_dof_trajectory)
def CarrotCb(self, data):
if self.carrot_received == False and self.carrot_status == "HOLD":
self.carrot_data = data
self.carrot_received = True
rospy.logwarn(
"Carrot received in position hold mode. Generating trajectory")
# Set up helical trajectory
helix_request = GetHelixPointsRequest()
helix_request.r = self.r
helix_request.angleStep = self.angleStep
helix_request.x0 = self.x0
helix_request.y0 = self.y0
helix_request.z0 = self.z0
helix_request.zf = self.zf
helix_request.deltaZ = self.deltaZ
# Get the points
helix_response = self.get_helix_points_service(helix_request)
# Prepend the current carrot position
first_point = JointTrajectoryPoint()
initial_pose = data.transforms[0].translation
first_point.positions = [
initial_pose.x, initial_pose.y, initial_pose.z, 0
]
helix_response.helix_points.points.insert(0, first_point)
# GetHelixPoints just returns the points. The dynamical part must be
# provided by the user.
dof = len(helix_response.helix_points.points[0].positions)
helix_response.helix_points.points[0].velocities = [
self.velocities
] * dof
helix_response.helix_points.points[0].accelerations = [
self.accelerations
] * dof
# Now call the trajectory generation service
trajectory_request = GenerateTrajectoryRequest()
trajectory_request.waypoints = helix_response.helix_points
trajectory_request.sampling_frequency = 100
trajectory_response = self.request_trajectory_service(
trajectory_request)
# Repack the received trajectory to MultiDof
self.multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory(
trajectory_response.trajectory)
self.trajectory_generated = True
rospy.loginfo("Trajectory generated")
self.trajectory_pub.publish(self.multi_dof_trajectory)
def __init__(self):
# First set up service
trajectory_type = rospy.get_param("~trajectory_type",
"generate_toppra_trajectory")
request_trajectory_service = rospy.ServiceProxy(
trajectory_type, GenerateTrajectory)
get_helix_points_service = rospy.ServiceProxy("get_helix_points",
GetHelixPoints)
trajectory_pub = rospy.Publisher('multi_dof_trajectory',
MultiDOFJointTrajectory,
queue_size=1)
time.sleep(0.5)
# Set up helical trajectory
helix_request = GetHelixPointsRequest()
helix_request.r = 0.5
helix_request.angleStep = 0.5
helix_request.x0 = -0.5
helix_request.y0 = 0.0
helix_request.z0 = 1.0
helix_request.zf = 2.5
helix_request.deltaZ = 0.05
# Get the points
helix_response = get_helix_points_service(helix_request)
# GetHelixPoints just returns the points. The dynamical part must be
# provided by the user.
dof = len(helix_response.helix_points.points[0].positions)
helix_response.helix_points.points[0].velocities = [2.0] * dof
helix_response.helix_points.points[0].accelerations = [1.0] * dof
# Now call the trajectory generation service
trajectory_request = GenerateTrajectoryRequest()
trajectory_request.waypoints = helix_response.helix_points
trajectory_request.sampling_frequency = 100
trajectory_response = request_trajectory_service(trajectory_request)
multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory(
trajectory_response.trajectory)
trajectory_pub.publish(multi_dof_trajectory)
def __init__(self):
# First set up service
request_trajectory_service = rospy.ServiceProxy(
"generate_toppra_trajectory", GenerateTrajectory)
# This is an example for UAV and output trajectory is converted
# accordingly
trajectory_pub = rospy.Publisher('multi_dof_trajectory',
MultiDOFJointTrajectory,
queue_size=1)
time.sleep(0.5)
# Example of a simple square trajectory for quadcopter. All vectors
# must be of same length
x = [0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0]
y = [0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0, 0, 1, 1, 0]
z = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1]
yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
# Another example. Same square defined through more points. This will
# overwrite the first example
x = [0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0, 0.0, 0.0]
y = [0.0, 0.0, 0.0, 0.5, 1.0, 1.0, 1.0, 0.5, 0.0]
z = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
yaw = [0, 0, 0, 0, 0, 0, 0, 0, 0]
# Create a service request which will be filled with waypoints
request = GenerateTrajectoryRequest()
# Add waypoints in request
waypoint = JointTrajectoryPoint()
for i in range(0, len(x)):
# Positions are defined above
waypoint.positions = [x[i], y[i], z[i], yaw[i]]
# Also add constraints for velocity and acceleration. These
# constraints are added only on the first waypoint since the
# TOPP-RA reads them only from there.
if i == 0:
waypoint.velocities = [2, 2, 2, 1]
waypoint.accelerations = [1.25, 1.25, 1.25, 1]
# Append all waypoints in request
request.waypoints.points.append(copy.deepcopy(waypoint))
# Set up joint names. This step is not necessary
request.waypoints.joint_names = ["x", "y", "z", "yaw"]
# Set up sampling frequency of output trajectory.
request.sampling_frequency = 100.0
# Set up number of gridpoints. The more gridpoints there are,
# trajectory interpolation will be more accurate but slower.
# Defaults to 100
request.n_gridpoints = 500
# If you want to plot Maximum Velocity Curve and accelerations you can
# send True in this field. This is intended to be used only when you
# have to debug something since it will block the service until plot
# is closed.
request.plot = False
# Request the trajectory
response = request_trajectory_service(request)
# Response will have trajectory and bool variable success. If for some
# reason the trajectory was not able to be planned or the configuration
# was incomplete or wrong it will return False.
print "Converting trajectory to multi dof"
joint_trajectory = response.trajectory
multi_dof_trajectory = self.JointTrajectory2MultiDofTrajectory(
joint_trajectory)
trajectory_pub.publish(multi_dof_trajectory)
def generateTrajectoryCallback(self, msg):
print("Generating trajectory")
# Uav + ugv for westpoint, circular trajectory
"""
x_uav = []
y_uav = []
z_uav = []
yaw_uav = []
x_ugv = []
y_ugv = []
amp_uav = 0.5
amp_ugv = 0.5
total_angle = 2*math.pi
n = 111
for i in range(0,n):
angle = i*total_angle/float(n-1)
x_uav.append(amp_uav*math.cos(angle))
y_uav.append(amp_uav*math.sin(angle))
z_uav.append(1.0)
yaw_uav.append(0.0)
x_ugv.append(amp_ugv*math.cos(angle))
y_ugv.append(amp_ugv*math.sin(angle))
for i in range(0,n-1):
dx = x_uav[i+1]-x_uav[i]
dy = y_uav[i+1]-y_uav[i]
yaw_uav[i] = math.atan2(dy, dx)
for i in range(1, n-1):
if yaw_uav[i] < yaw_uav[i-1]:
yaw_uav[i] = yaw_uav[i] + 2*math.pi
yaw_uav[n-1] = yaw_uav[n-2]
"""
# Uav + ugv + manipulator trajectory
x_uav = []
y_uav = []
z_uav = []
yaw_uav = []
x_ugv = []
y_ugv = []
x_start = -0.5
x_end = 0.5
y_start = 0.0
y_end = 0.0
x_offset = 0.2597
y_offset = -0.1777
z_offset = -0.22
yaw_offset = -math.pi/2.0
n = 111
for i in range(0,n):
dx = x_end - x_start
dy = y_end - y_start
x_uav.append(dx*float(i)/float(n)+x_start)
y_uav.append(dy*float(i)/float(n)+y_start)
z_uav.append(1.0)
yaw_uav.append(0.0)
x_ugv.append(dx*float(i)/float(n)+x_start)
y_ugv.append(dy*float(i)/float(n)+y_start)
for i in range(0,n-1):
dx = x_uav[i+1]-x_uav[i]
dy = y_uav[i+1]-y_uav[i]
yaw_uav[i] = math.atan2(dy, dx)
for i in range(1, n-1):
if yaw_uav[i] < yaw_uav[i-1]:
yaw_uav[i] = yaw_uav[i] + 2*math.pi
yaw_uav[n-1] = yaw_uav[n-2]
# Create a service request which will be filled with waypoints
request = GenerateTrajectoryRequest()
waypoint = JointTrajectoryPoint()
for i in range(0, n):
waypoint.positions = [x_uav[i], y_uav[i], z_uav[i], yaw_uav[i], x_ugv[i], y_ugv[i], (x_uav[i]+x_offset), (y_uav[i]+y_offset), (z_uav[i]+z_offset), (yaw_uav[i]+yaw_offset)]
waypoint.velocities = [1.2, 1.2, 1.2, 4.6, 0.25, 0.25, 1.2, 1.2, 1.2, 4.6]
waypoint.accelerations = [0.8, 0.8, 0.8, 4.0, 0.04, 0.04, 0.8, 0.8, 0.8, 4.0]
request.waypoints.points.append(copy.deepcopy(waypoint))
request.waypoints.joint_names = ["x_uav", "y_uav", "z_uav", "yaw_uav", "x_ugv", "y_ugv", "x_man", "y_man", "z_man", "yaw_man"]
request.sampling_frequency = 100.0
response = self.request_trajectory_service(request)
print("Total trajectory time: ", len(response.trajectory.points)/request.sampling_frequency)
print("Converting trajectory to multi dof")
joint_trajectory = response.trajectory
multi_dof_trajectory = self.jointTrajectory2MultiDofTrajectory(joint_trajectory)
self.trajectory_pub.publish(multi_dof_trajectory)
ugv_trajectory = self.jointTrajectory2MultiDofTrajectoryUgv(joint_trajectory)
self.ugv_trajectory_pub.publish(ugv_trajectory)
manipulator_trajectory = self.jointTrajectory2MultiDofTrajectoryManipulator(joint_trajectory)
self.manipulator_trajectory_pub.publish(manipulator_trajectory)
if msg.data == 1:
self.plotTrajectory(request, response)
def trajectory_cb(self, msg):
if len(msg.points) == 0:
print("ToppTracker - empty input trajectory recieved, RESET")
self.trajectory = MultiDOFJointTrajectory()
return
if not self.carrot_trajectory_recieved:
print("ToppTracker - trajectory recieved but carrot unavailable")
self.trajectory = MultiDOFJointTrajectory()
return
# Nicely interpolate points from current to first
x, y, z, yaw = self.interpolate_points(self.carrot_trajectory,
msg.points[0])
print(x)
print(y)
print(yaw)
if len(x) == 0:
x.append(self.carrot_trajectory.transforms[0].translation.x)
y.append(self.carrot_trajectory.transforms[0].translation.y)
z.append(self.carrot_trajectory.transforms[0].translation.z)
yaw.append(
tf.transformations.euler_from_quaternion([
self.carrot_trajectory.transforms[0].rotation.x,
self.carrot_trajectory.transforms[0].rotation.y,
self.carrot_trajectory.transforms[0].rotation.z,
self.carrot_trajectory.transforms[0].rotation.w
])[2])
for point in msg.points:
x.append(point.transforms[0].translation.x)
y.append(point.transforms[0].translation.y)
z.append(point.transforms[0].translation.z)
yaw.append(
tf.transformations.euler_from_quaternion([
point.transforms[0].rotation.x,
point.transforms[0].rotation.y,
point.transforms[0].rotation.z,
point.transforms[0].rotation.w
])[2])
# Fix Toppra orientation, at this point atleast two points are in trajectory
if len(yaw) > 1:
yaw[-1] = self.fix_topp_yaw(yaw[-1], yaw[-2])
for x_, y_, z_, yaw_ in zip(x, y, z, yaw):
print("Recieved point: ", x_, y_, z_, yaw_)
request = GenerateTrajectoryRequest()
# Add waypoints in request
waypoint = JointTrajectoryPoint()
for i in range(0, len(x)):
# Positions are defined above
waypoint.positions = [x[i], y[i], z[i], yaw[i]]
# Also add constraints for velocity and acceleration. These
# constraints are added only on the first waypoint since the
# TOPP-RA reads them only from there.
if i == 0:
waypoint.velocities = [
self.tracker_params.velocity[0],
self.tracker_params.velocity[1],
self.tracker_params.velocity[2],
self.tracker_params.velocity[3]
]
waypoint.accelerations = [
self.tracker_params.acceleration[0],
self.tracker_params.acceleration[1],
self.tracker_params.acceleration[2],
self.tracker_params.acceleration[3]
]
# Append all waypoints in request
request.waypoints.points.append(copy.deepcopy(waypoint))
request.waypoints.joint_names = ["x", "y", "z", "yaw"]
request.sampling_frequency = self.tracker_params.sampling_frequency
request.n_gridpoints = self.tracker_params.n_gridpoints
request.plot = False
# Request the trajectory
request_trajectory_service = rospy.ServiceProxy(
"generate_toppra_trajectory", GenerateTrajectory)
response = request_trajectory_service(request)
# Response will have trajectory and bool variable success. If for some
# reason the trajectory was not able to be planned or the configuration
# was incomplete or wrong it will return False.
print("ToppTracker: Converting trajectory to multi dof")
joint_trajectory = response.trajectory
self.enable_trajectory = False
# Take the first point in the message, extract its roll / pitch and copy it in the
# final trajectory
angles = tf.transformations.euler_from_quaternion([
msg.points[0].transforms[0].rotation.x,
msg.points[0].transforms[0].rotation.y,
msg.points[0].transforms[0].rotation.z,
msg.points[0].transforms[0].rotation.w
])
self.trajectory = self.JointTrajectory2MultiDofTrajectory(
joint_trajectory, angles[0], angles[1])
# Publish the path
path_msg = Path()
path_msg.header.stamp = rospy.Time.now()
path_msg.header.frame_id = msg.header.frame_id
self.trajectory_pose_arr.header.stamp = rospy.Time.now()
self.trajectory_pose_arr.header.frame_id = msg.header.frame_id
self.trajectory_pose_arr.poses = []
self.trajectory_index = 0
for i, point in enumerate(self.trajectory.points):
path_point = PoseStamped()
path_point.header.stamp = rospy.Time.now()
path_point.header.frame_id = msg.header.frame_id
path_point.pose.position.x = point.transforms[0].translation.x
path_point.pose.position.y = point.transforms[0].translation.y
path_point.pose.position.z = point.transforms[0].translation.z
path_point.pose.orientation.x = point.transforms[0].rotation.x
path_point.pose.orientation.y = point.transforms[0].rotation.y
path_point.pose.orientation.z = point.transforms[0].rotation.z
path_point.pose.orientation.w = point.transforms[0].rotation.w
path_msg.poses.append(path_point)
if i % 10 != 0:
continue
self.trajectory_pose_arr.poses.append(path_point.pose)
self.path_pub.publish(path_msg)