def JointTrajectory2MultiDofTrajectory(self, joint_trajectory):
multi_dof_trajectory = MultiDOFJointTrajectory()
for i in range(0, len(joint_trajectory.points)):
temp_point = MultiDOFJointTrajectoryPoint()
temp_transform = Transform()
temp_transform.translation.x = joint_trajectory.points[
i].positions[0]
temp_transform.translation.y = joint_trajectory.points[
i].positions[1]
temp_transform.translation.z = joint_trajectory.points[
i].positions[2]
temp_transform.rotation.w = 1.0
temp_vel = Twist()
temp_vel.linear.x = joint_trajectory.points[i].velocities[0]
temp_vel.linear.y = joint_trajectory.points[i].velocities[1]
temp_vel.linear.z = joint_trajectory.points[i].velocities[2]
temp_acc = Twist()
temp_acc.linear.x = joint_trajectory.points[i].accelerations[0]
temp_acc.linear.y = joint_trajectory.points[i].accelerations[1]
temp_acc.linear.z = joint_trajectory.points[i].accelerations[2]
temp_point.transforms.append(temp_transform)
temp_point.velocities.append(temp_vel)
temp_point.accelerations.append(temp_acc)
temp_point.time_from_start = joint_trajectory.points[
i].time_from_start
multi_dof_trajectory.points.append(temp_point)
return multi_dof_trajectory
def generate_trajectory(self):
traj_to_execute = MultiDOFJointTrajectory()
traj_header = std_msgs.msg.Header()
traj_velocities = Twist()
traj_accelerations = Twist()
traj_to_execute.joint_names = ["virtual_joint"]
for i in range(0, 5):
traj_header.stamp = self.get_clock().now().to_msg(
) #rclpy.time.Time().msg()
traj_to_execute.header = traj_header
#traj_quaternion = tf2.transformations.quaternion_from_euler(roll,pitch,yaw)
traj_quaternion = [0.0, 0.0, 0.0, 0.0]
traj_transforms = Transform()
traj_transforms.translation.x = 1.0 * i
traj_transforms.translation.y = 2.0 * i
traj_transforms.translation.z = 3.0 * i
traj_transforms.rotation = Quaternion()
traj_transforms.rotation.x = traj_quaternion[0]
traj_transforms.rotation.y = traj_quaternion[1]
traj_transforms.rotation.z = traj_quaternion[2]
traj_transforms.rotation.w = traj_quaternion[3]
point_i = MultiDOFJointTrajectoryPoint()
point_i.transforms.append(traj_transforms)
point_i.velocities.append(traj_velocities)
point_i.accelerations.append(traj_accelerations)
duration_i = Duration(seconds=1.0).to_msg()
point_i.time_from_start = duration_i # self.get_clock().now().to_msg()+Duration(seconds=1.0)
traj_to_execute.points.append(point_i)
return traj_to_execute
def publish_waypoint(x, y, z, yaw):
"""
Publish a waypoint to
"""
command_publisher = rospy.Publisher('/iris/command/trajectory',
MultiDOFJointTrajectory,
queue_size=10)
# create trajectory msg
traj = MultiDOFJointTrajectory()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = 'frame'
traj.joint_names.append('base_link')
# create start point for trajectory
transforms = Transform()
transforms.translation.x = 0
transforms.translation.y = 0
transforms.translation.z = 0
quat = tf.transformations.quaternion_from_euler(yaw * np.pi / 180.0,
0,
0,
axes='rzyx')
transforms.rotation.x = quat[0]
transforms.rotation.y = quat[1]
transforms.rotation.z = quat[2]
transforms.rotation.w = quat[3]
velocities = Twist()
accel = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel],
rospy.Time(2))
traj.points.append(point)
# create end point for trajectory
transforms = Transform()
transforms.translation.x = x
transforms.translation.y = y
transforms.translation.z = z
quat = tf.transformations.quaternion_from_euler((yaw) * np.pi / 180.0,
0,
0,
axes='rzyx')
transforms.rotation.x = quat[0]
transforms.rotation.y = quat[1]
transforms.rotation.z = quat[2]
transforms.rotation.w = quat[3]
velocities = Twist()
accel = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accel],
rospy.Time(2))
traj.points.append(point)
rospy.sleep(1)
command_publisher.publish(traj)
def producing_message(pos, vel, acc, t_disc, newMsg):
"""
This function produces message to publish
from pos, vel, acc.
pos - tuple of lists (x, y, z) x: list of all x positions in trajectory
vel - tuple of lists (v_x, v_y, v_z) v_x: list of all x positions in trajectory
acc - tuple of lists (a_x, a_y, a_z) a_x: list of all x accelerations in trajectory
t_disc - list (time)
newMsg - already created (empty?) message
"""
x, y, z = pos
v_x, v_y, v_z = vel
a_x, a_y, a_z = acc
for idx, t in enumerate(t_disc):
# initialization of message components
newPoint = MultiDOFJointTrajectoryPoint()
newTransform = Transform()
newQuaternion = Quaternion()
newVelocities = Twist()
newAccelerations = Twist()
newTransform.translation.x = x[idx]
newTransform.translation.y = y[idx]
newTransform.translation.z = z[idx]
# appending position in this point
newPoint.transforms.append(newTransform)
newVelocities.linear.x = v_x[idx]
newVelocities.linear.y = v_y[idx]
newVelocities.linear.z = v_z[idx]
# appending linear velocities in this point
newPoint.velocities.append(newVelocities)
newAccelerations.linear.x = a_x[idx]
newAccelerations.linear.y = a_y[idx]
newAccelerations.linear.z = a_z[idx]
# appending linear accelarations in this point
newPoint.accelerations.append(newAccelerations)
# time from start in seconds
newPoint.time_from_start = rospy.Duration(t)
newMsg.points.append(newPoint)
return newMsg
def emergency_response(self, action):
pub_action = Point()
# IN FUTURE CHANGE TO RELATIVE
if abs(action.y - self.pose_y) < 0.05:
pub_action.x = 5
pub_action.y = 0
pub_action.z = 1.5
else:
pub_action = action
wpt = MultiDOFJointTrajectory()
header = Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
wpt.joint_names.append('base_link')
wpt.header = header
quat = quaternion.from_euler_angles(0, 0, math.radians(-45))
#
# if self.active == 'astar':
# transforms = Transform(translation=self.astar, rotation=quat)
# elif self.active == 'dnn':
# transforms = Transform(translation=self.dnn, rotation=quat)
# else:
# print('No Choice Made!')
# transforms = Transform()
print('Selected {}'.format(self.active))
transforms = Transform(translation=pub_action, rotation=quat)
velocities = Twist()
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations], rospy.Time(0))
wpt.points.append(point)
self.waypoint_pub.publish(wpt)
def hover_the_quad(self):
"""give commands to hover at 1m above the starting position"""
self.RHP_time = time.time()-self.hovering_time
#custom_traj = PolynomialTrajectory()
#custom_traj.header.stamp = rospy.Time.now()
#custom_traj.pdes.x = -6.5; custom_traj.pdes.y = -4.0; custom_traj.pdes.z = 2.0
#custom_traj.vdes.x = 0; custom_traj.vdes.y = 0; custom_traj.vdes.z = 0
#custom_traj.ades.x = 0; custom_traj.ades.y = 0; custom_traj.ades.z = 0
#custom_traj.ddes.x = 1.0; custom_traj.ddes.y = 0.0; custom_traj.ddes.z = 0.0
#custom_traj.controller = 0; custom_traj.time = self.RHP_time
#self.custom_traj_pub.publish(custom_traj)
# for now angular acceleration Point is used to specify the direction
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'frame'
traj.header = header
traj.joint_names = 'nothing'
#print self.initial_position, self.initial_orientation, self.hovering_height
transforms = Transform(translation = Point(self.initial_position[0], self.initial_position[1], self.hovering_height), rotation = Quaternion(0,0,0,1))
velocities = Twist(linear = Point(0, 0, 0), angular = Point(0,0,0))
accelerations = Twist(linear = Point(0, 0, 0), angular = Point(self.initial_orientation[0],self.initial_orientation[1],self.initial_orientation[2]))
point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time))
traj.points.append(point)
self.uav_traj_pub.publish(traj)
def jointTrajectoryPointToMultiDofJointTrajectoryPoint(joint):
multi = MultiDOFJointTrajectoryPoint()
transform = Transform()
transform.translation.x = joint.positions[0]
transform.translation.y = joint.positions[1]
transform.translation.z = joint.positions[2]
transform.rotation.z = math.sin(joint.positions[3 + 2] / 2.0)
transform.rotation.w = math.cos(joint.positions[3 + 2] / 2.0)
vel = Twist()
vel.linear.x = joint.velocities[0]
vel.linear.y = joint.velocities[1]
vel.linear.z = joint.velocities[2]
acc = Twist()
acc.linear.x = joint.accelerations[0]
acc.linear.y = joint.accelerations[1]
acc.linear.z = joint.accelerations[2]
multi.transforms.append(transform)
multi.velocities.append(vel)
multi.accelerations.append(acc)
return multi
def jointTrajectory2MultiDofTrajectoryUgv(self, joint_trajectory):
multi_dof_trajectory = MultiDOFJointTrajectory()
for i in range(0, len(joint_trajectory.points)):
temp_point = MultiDOFJointTrajectoryPoint()
temp_transform = Transform()
temp_transform.translation.x = joint_trajectory.points[i].positions[4]
temp_transform.translation.y = joint_trajectory.points[i].positions[5]
temp_transform.translation.z = 0.0
temp_transform.rotation.z = math.sin(joint_trajectory.points[i].positions[3]/2.0)
temp_transform.rotation.w = math.cos(joint_trajectory.points[i].positions[3]/2.0)
temp_vel = Twist()
temp_vel.linear.x = joint_trajectory.points[i].velocities[4]
temp_vel.linear.y = joint_trajectory.points[i].velocities[5]
temp_vel.linear.z = 0.0
temp_acc = Twist()
temp_acc.linear.x = joint_trajectory.points[i].accelerations[4]
temp_acc.linear.y = joint_trajectory.points[i].accelerations[5]
temp_acc.linear.z = 0.0
temp_point.transforms.append(temp_transform)
temp_point.velocities.append(temp_vel)
temp_point.accelerations.append(temp_acc)
multi_dof_trajectory.points.append(temp_point)
return multi_dof_trajectory
def publish_command(position,velocity):
rospy.init_node('goto_poition',anonymous=True)
firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory',MultiDOFJointTrajectory,queue_size=10)
desired_yaw = -10
desired_x = position[0]
desired_y = position[1]
desired_z = position[2]
quaternion = tf.transformations.quaternion_from_euler(0,0,math.radians(desired_yaw))
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
traj.joint_names.append('base_link')
traj.header = header
transform = Transform(translation=Point(desired_x,desired_y,desired_z),rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
velocities = Twist()
velocities.linear.x = velocity[0]
velocities.linear.y = velocity[1]
velocities.linear.z = velocity[2]
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transform],[velocities],[accelerations],rospy.Time(2))
traj.points.append(point)
time.sleep(3)
firefly_command_publisher.publish(traj)
rospy.loginfo("Have published %s into %s!",position+velocity,'/firefly/command/trajectory')
def getCommandFromTrajectory(self): command = MultiDOFJointTrajectory() point = MultiDOFJointTrajectoryPoint() transform = Transform() twist = Twist() point.transforms.append(copy.deepcopy(transform)) point.velocities.append(copy.deepcopy(twist)) point.accelerations.append(copy.deepcopy(twist)) command.points.append(copy.deepcopy(point)) if (self.new_trajectory): if (self.trajectory_index < self.trajectory_length): self.current_pose_reference.position.x = self.trajectory_reference.points[self.trajectory_index].transforms[0].translation.x self.current_pose_reference.position.y = self.trajectory_reference.points[self.trajectory_index].transforms[0].translation.y self.current_pose_reference.orientation.x = self.trajectory_reference.points[self.trajectory_index].transforms[0].rotation.x self.current_pose_reference.orientation.y = self.trajectory_reference.points[self.trajectory_index].transforms[0].rotation.y self.current_pose_reference.orientation.z = self.trajectory_reference.points[self.trajectory_index].transforms[0].rotation.z self.current_pose_reference.orientation.w = self.trajectory_reference.points[self.trajectory_index].transforms[0].rotation.w self.trajectory_index += int(self.trajectory_rate/self.rate) else: self.new_trajectory = False command.points[0].transforms[0].translation.x = self.current_pose_reference.position.x command.points[0].transforms[0].translation.y = self.current_pose_reference.position.y command.points[0].transforms[0].rotation.x = self.current_pose_reference.orientation.x command.points[0].transforms[0].rotation.y = self.current_pose_reference.orientation.y command.points[0].transforms[0].rotation.z = self.current_pose_reference.orientation.z command.points[0].transforms[0].rotation.w = self.current_pose_reference.orientation.w return command
def publish_pose_speed_command(self, x, y, z, rx, ry, rz, speed_vector):
print('Command pose: '+self._namespace, x, y, z, rx, ry, rz, speed_vector )
quaternion = tf.transformations.quaternion_from_euler(rx, ry, rz)
rotation = Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3])
location = Point(x, y, z)
transforms = Transform(translation=location,
rotation=rotation)
velocities = Twist()
velocities.linear = speed_vector
accelerations = Twist()
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
traj.joint_names.append('base_link')
traj.header = header
point = MultiDOFJointTrajectoryPoint([transforms], [velocities], [accelerations], rospy.Time(0))
traj.points.append(point)
self.command_pub.publish(traj)
def traj_update(self):
self.msg.header.stamp = rospy.Time.now()
for i in range(0, self.future_length):
velocities = Twist()
accelerations = Twist()
x = self.x_o + self.mag * math.sin(self.speed * (
(self.t - self.t_o) + i * self.dt))
y = self.y_o + self.mag * math.cos(self.speed * (
(self.t - self.t_o) + i * self.dt))
z = self.z_o
velocities.linear.x = self.speed * self.mag * math.cos(
self.speed * ((self.t - self.t_o) + i * self.dt))
velocities.linear.y = self.speed * -self.mag * math.sin(
self.speed * ((self.t - self.t_o) + i * self.dt))
velocities.linear.z = 0.0
accelerations.linear.x = self.speed * self.speed * -self.mag * math.sin(
self.speed * ((self.t - self.t_o) + i * self.dt))
accelerations.linear.y = self.speed * self.speed * -self.mag * math.cos(
self.speed * ((self.t - self.t_o) + i * self.dt))
accelerations.linear.z = 0.0
transforms = Transform(translation=Point(x, y, z),
rotation=Quaternion(self.quaternion[0],
self.quaternion[1],
self.quaternion[2],
self.quaternion[3]))
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Time(i * self.dt))
self.msg.points[i] = point
return self.msg
def stepCb(self, data):
if self.carrot_received:
print self.carrot_pose.position.x + data.pose.position.x
new_msg = MultiDOFJointTrajectoryPoint()
new_msg.transforms.append(Transform())
new_msg.velocities.append(Twist())
new_msg.accelerations.append(Twist())
new_msg.transforms[
0].translation.x = self.carrot_pose.position.x + data.pose.position.x
new_msg.transforms[
0].translation.y = self.carrot_pose.position.y + data.pose.position.y
new_msg.transforms[
0].translation.z = self.carrot_pose.position.z + data.pose.position.z
new_msg.transforms[
0].rotation.x = self.carrot_pose.orientation.x + data.pose.orientation.x
new_msg.transforms[
0].rotation.y = self.carrot_pose.orientation.y + data.pose.orientation.y
new_msg.transforms[
0].rotation.z = self.carrot_pose.orientation.z + data.pose.orientation.z
new_msg.transforms[
0].rotation.w = self.carrot_pose.orientation.w + data.pose.orientation.w
print new_msg
self.publisher.publish(new_msg)
def __init__(self):
# UAV publishers for trajectory
self.uav_trajectory_point_pub = rospy.Publisher(
'trajectory_point_ref', MultiDOFJointTrajectoryPoint, queue_size=1)
self.executing_trajectory_pub = rospy.Publisher('executing_trajectory',
Int32,
queue_size=1)
# Manipulator publishers
self.manipulator_joint1_pub = rospy.Publisher(
'joint1_position_controller/command', Float64, queue_size=1)
self.manipulator_joint2_pub = rospy.Publisher(
'joint2_position_controller/command', Float64, queue_size=1)
self.manipulator_joint3_pub = rospy.Publisher(
'joint3_position_controller/command', Float64, queue_size=1)
self.manipulator_joint4_pub = rospy.Publisher(
'joint4_position_controller/command', Float64, queue_size=1)
self.manipulator_joint5_pub = rospy.Publisher(
'joint5_position_controller/command', Float64, queue_size=1)
rospy.Subscriber('joint_trajectory',
JointTrajectory,
self.jointTrajectoryCallback,
queue_size=1)
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate)
self.t_start = rospy.Time.now()
self.executing_trajectory_flag = False
self.trajectory = JointTrajectory()
self.current_trajectory_point = JointTrajectoryPoint()
self.uav_current_trajectory_point = MultiDOFJointTrajectoryPoint()
def fastPlannerTrajCallback(self, msg):
# position and yaw
pose = Transform()
pose.translation.x = msg.position.x
pose.translation.y = msg.position.y
pose.translation.z = msg.position.z
q = quaternion_from_euler(0, 0, msg.yaw) # RPY
pose.rotation.x = q[0]
pose.rotation.y = q[1]
pose.rotation.z = q[2]
pose.rotation.w = q[3]
# velocity
vel = Twist()
vel.linear = msg.velocity
# TODO: set vel.angular to msg.yaw_dot
# acceleration
acc = Twist()
acc.linear = msg.acceleration
traj_point = MultiDOFJointTrajectoryPoint()
traj_point.transforms.append(pose)
traj_point.velocities.append(vel)
traj_point.accelerations.append(acc)
traj_msg = MultiDOFJointTrajectory()
traj_msg.header = msg.header
traj_msg.points.append(traj_point)
self.traj_pub.publish(traj_msg)
def traj_update(self):
self.msg.header.stamp = rospy.Time.now()
for i in range(0, self.future_length):
velocities = Twist()
accelerations = Twist()
# needed just in case the array bounds are overflowed
j = self.array_position + i * 5
if j >= self.array_length:
j = self.t_o * self.hz
x = self.x_array[j]
y = self.y_array[j]
z = self.z_array[j]
velocities.linear.x = self.vx_array[j]
velocities.linear.y = self.vy_array[j]
velocities.linear.z = self.vz_array[j]
accelerations.linear.x = 0.0
accelerations.linear.y = 0.0
accelerations.linear.z = 0.0
transforms = Transform(translation=Point(x, y, z),
rotation=Quaternion(self.quaternion[0],
self.quaternion[1],
self.quaternion[2],
self.quaternion[3]))
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Time(i * self.dt))
self.msg.points[i] = point
return self.msg
def UpdateAndPublish(self, x_tilde, y_tilde, z_tilde, vx_tilde, vy_tilde,
vz_tilde, ax_tilde, ay_tilde, az_tilde):
self.msg.header.stamp = rospy.Time.now()
#to = rospy.Time.now()
for i in range(0, self.future_length):
velocities = Twist()
accelerations = Twist()
x = x_tilde[i]
y = y_tilde[i]
z = z_tilde[i]
velocities.linear.x = vx_tilde[i]
velocities.linear.y = vy_tilde[i]
velocities.linear.z = vz_tilde[i]
accelerations.linear.x = ax_tilde[i]
accelerations.linear.y = ay_tilde[i]
accelerations.linear.z = az_tilde[i]
transforms = Transform(translation=Point(x, y, z),
rotation=Quaternion(self.quaternion[0],
self.quaternion[1],
self.quaternion[2],
self.quaternion[3]))
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Time(i * self.dt))
self.msg.points[i] = point
self.traj_pub.publish(self.msg)
def hover_the_quad(self):
"""give commands to hover at 1m above the starting position"""
self.RHP_time = time.time() - self.hovering_time
# for now angular acceleration Point is used to specify the direction
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world'
traj.header = header
traj.joint_names = 'firefly/base_link'
#print self.initial_position, self.initial_orientation, self.hovering_height
transforms = Transform(translation=Point(self.initial_position[0],
self.initial_position[1],
self.hovering_height),
rotation=Quaternion(0, 0, 0, 1))
velocities = Twist(linear=Point(0, 0, 0), angular=Point(0, 0, 0))
accelerations = Twist(linear=Point(0, 0, 0),
angular=Point(self.initial_orientation[0],
self.initial_orientation[1],
self.initial_orientation[2]))
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Duration(self.RHP_time))
traj.points.append(point)
self.uav_traj_pub.publish(traj)
def __init__(self):
self.pub = rospy.Publisher('multi_dof_trajectory',
MultiDOFJointTrajectory,
queue_size=1)
kvat = Quaternion(0, 0, 0, 1)
self.acc = Twist()
self.acc.angular = Vector3(0, 0, 0)
self.vel = Twist()
self.vel.angular = Vector3(0, 0, 0)
self.pos = Transform()
self.pos.rotation = kvat
self.traj_point = MultiDOFJointTrajectoryPoint()
self.traj = MultiDOFJointTrajectory()
self.pose = PoseStamped()
self.positions = []
self.trajectory = []
self.time = []
self.traj_point.transforms = []
self.traj_point.accelerations = []
self.traj_point.velocities = []
self.traj.joint_names = ['UAV']
h = Header()
h.stamp = rospy.Time.now()
self.traj.header = h
self.traj.points = []
self.rate = rospy.get_param('~rate', 100)
self.ros_rate = rospy.Rate(self.rate)
def callback(data):
rospy.loginfo("Current position:%s,%s,%s", data.point.x,data.point.y,data.point.z)
R1.location_x, R1.location_y, R1.location_z = position[0], position[1], position[2]
currrent_x, currrent_y = R1.location_x, R1.location_y
target_x, target_y = R1.target_x, R2.target_y
position = [round(data.point.x) + 1, round(data.point.y) + 1, round(data.point.z)+1]
velocity = [0,0,0]
desired_yaw = -10
desired_x = position[0]
desired_y = position[1]
desired_z = position[2]
quaternion = tf.transformations.quaternion_from_euler(0,0,math.radians(desired_yaw))
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
traj.joint_names.append('base_link')
traj.header = header
transform = Transform(translation=Point(desired_x,desired_y,desired_z),rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
velocities = Twist()
velocities.linear.x = velocity[0]
velocities.linear.y = velocity[1]
velocities.linear.z = velocity[2]
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transform],[velocities],[accelerations],rospy.Time(2))
traj.points.append(point)
firefly_command_publisher.publish(traj)
rospy.loginfo("Have published %s into %s!",position+velocity,'/firefly/command/trajectory')
def __init__(self):
self.trajectory_point_pub = rospy.Publisher(
'trajectory_point_ref', MultiDOFJointTrajectoryPoint, queue_size=1)
self.executing_trajectory_pub = rospy.Publisher('executing_trajectory',
Int32,
queue_size=1)
rospy.Subscriber('multi_dof_trajectory',
MultiDOFJointTrajectory,
self.multi_dof_trajectory_cb,
queue_size=1)
rospy.Subscriber('stop_trajectory_execution',
Empty,
self.stop_trajectory_execution_cb,
queue_size=1)
rospy.Subscriber('trajectory_type',
Int32,
self.trajectory_type_cb,
queue_size=1)
self.rate = rospy.get_param('~rate', 100)
self.trajectory_type = rospy.get_param('~trajectory_type', 0)
self.split_start = rospy.get_param('~split_start', 5.21) #11.13
self.split_end = rospy.get_param('~split_end', 9.59) #16.85
self.ros_rate = rospy.Rate(self.rate)
self.t_start = rospy.Time.now()
self.executing_trajectory_flag = False
self.trajectory = MultiDOFJointTrajectory()
self.current_trajectory_point = MultiDOFJointTrajectoryPoint()
self.endless_trajectory = MultiDOFJointTrajectory()
def update(self, data):
self.traj = MultiDOFJointTrajectory()
self.traj.header.frame_id = ''
self.traj.header.stamp = rospy.Time.now()
self.traj.joint_names = ["base_link"]
transforms = Transform()
transforms.translation.x = data.position.x
transforms.translation.y = data.position.y
transforms.translation.z = data.position.z
transforms.rotation.x = data.orientation.x
transforms.rotation.y = data.orientation.y
transforms.rotation.z = data.orientation.z
transforms.rotation.w = data.orientation.w
velocities = Twist()
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Duration(0.1))
self.traj.points.append(point)
self.traj_pub.publish(self.traj)
rospy.loginfo('data post')
self.r.sleep()
return
def ros_publish_waypoint(self, action):
"""Publish single-point ROS trajectory message with given x, y and default z, att."""
# create trajectory msg
traj = MultiDOFJointTrajectory()
traj.header.stamp = rospy.Time.now()
traj.header.frame_id = 'frame'
traj.joint_names.append('base_link')
# create end point for trajectory
transforms = Transform()
transforms.translation.x = action[0]
transforms.translation.y = action[1]
transforms.translation.z = Z
quat = tf.transformations.quaternion_from_euler(0, 0, 0, axes='rzyx')
transforms.rotation.x = quat[0]
transforms.rotation.y = quat[1]
transforms.rotation.z = quat[2]
transforms.rotation.w = quat[3]
velocities = Twist()
accel = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accel], rospy.Time())
traj.points.append(point)
self.waypoint_publisher.publish(traj)
def update(self):
#self.msg = MultiDOFJointTrajectory()
rospy.loginfo(self.msg.points)
for i in range(0, self.length):
self.header.stamp = rospy.get_time()
self.msg.header = self.header
x = i + 1
y = i + 2
z = i + 3
transforms = Transform(translation=Point(x, y, z),
rotation=Quaternion(self.quaternion[0],
self.quaternion[1],
self.quaternion[2],
self.quaternion[3]))
p = MultiDOFJointTrajectoryPoint([transforms], [self.velocities],
[self.accelerations],
rospy.Time(i * self.dt))
rospy.loginfo(i)
self.msg.points[i] = p
#self.msg.points.append(p)
#self.msg.points(i).position.y
#self.msg.points(i).positions.z
#rospy.loginfo('Im printing nerd')
rospy.loginfo('')
rospy.loginfo(self.dt)
rospy.loginfo('New Point %f', self.count)
rospy.loginfo(self.msg)
self.count = self.count + 1
return self.msg
def JointTrajectory2MultiDofTrajectory(self,
joint_trajectory,
roll=0,
pitch=0):
multi_dof_trajectory = MultiDOFJointTrajectory()
for i in range(0, len(joint_trajectory.points)):
temp_point = MultiDOFJointTrajectoryPoint()
temp_transform = Transform()
temp_transform.translation.x = joint_trajectory.points[
i].positions[0]
temp_transform.translation.y = joint_trajectory.points[
i].positions[1]
temp_transform.translation.z = joint_trajectory.points[
i].positions[2]
quaternion = tf.transformations.quaternion_from_euler(
roll, pitch, joint_trajectory.points[i].positions[3])
temp_transform.rotation.x = quaternion[0]
temp_transform.rotation.y = quaternion[1]
temp_transform.rotation.z = quaternion[2]
temp_transform.rotation.w = quaternion[3]
temp_vel = Twist()
temp_vel.linear.x = joint_trajectory.points[i].velocities[0]
temp_vel.linear.y = joint_trajectory.points[i].velocities[1]
temp_vel.linear.z = joint_trajectory.points[i].velocities[2]
temp_acc = Twist()
temp_acc.linear.x = joint_trajectory.points[i].accelerations[0]
temp_acc.linear.y = joint_trajectory.points[i].accelerations[1]
temp_acc.linear.z = joint_trajectory.points[i].accelerations[2]
temp_point.transforms.append(temp_transform)
temp_point.velocities.append(temp_vel)
temp_point.accelerations.append(temp_acc)
temp_point.time_from_start = joint_trajectory.points[
i].time_from_start
multi_dof_trajectory.points.append(temp_point)
return multi_dof_trajectory
def talker():
rospy.init_node('traj_commander', anonymous=True)
pub = rospy.Publisher('/carla/ego_vehicle/desired_waypoints',
MultiDOFJointTrajectory,
queue_size=10)
viz_pub = rospy.Publisher('viz/desired_waypoints', Marker, queue_size=10)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
msg = MultiDOFJointTrajectory()
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = '/map'
viz_msg = Marker()
viz_msg.header.stamp = rospy.Time.now()
viz_msg.header.frame_id = '/map'
viz_msg.type = Marker.CUBE_LIST
viz_msg.scale.x = 1
viz_msg.scale.y = 1
viz_msg.scale.z = 1
viz_msg.color.a = 1
viz_msg.points = []
pts = []
num_pt = 30
radius = 19.5
ang_inc = 2.0 * math.pi / num_pt
for i in range(num_pt):
ang = ang_inc * i
pt = MultiDOFJointTrajectoryPoint()
pt.transforms = [Transform(Vector3(radius * math.cos(ang) - 0.5, radius * math.sin(ang) - 0.3, 0), \
Quaternion(0,0,0,1))]
pt.velocities = [Twist(Vector3(6, 0, 0), Vector3(0, 0, 0))]
viz_msg.points.append(
Point(radius * math.cos(ang), radius * math.sin(ang), 0))
pts.append(pt)
msg.points = pts
pub.publish(msg)
viz_pub.publish(viz_msg)
rate.sleep()
def create_multi_dof_joint_trajectory_msg(npts):
trajectory_msg = MultiDOFJointTrajectory()
trajectory_point = MultiDOFJointTrajectoryPoint()
trajectory_point.transforms.append(Transform())
trajectory_point.velocities.append(Twist())
trajectory_point.accelerations.append(Twist())
for i in range(npts):
trajectory_msg.points.append(trajectory_point)
trajectory_msg.joint_names.append('')
return trajectory_msg
def callback(odometry):
global path
global th0
current_time = rospy.get_rostime()
pelican_poses = PoseStamped()
# pelican_poses.pose = odometry.pose.pose
# pelican_poses.header.stamp = current_time
# pelican_poses.header.frame_id = 'world'
# path.poses.append(pelican_poses)
th0 = th0 + theta
rospy.loginfo("Current angle %s", th0)
x_current = odometry.pose.pose.position.x
y_current = odometry.pose.pose.position.y
x_new = 7.0 * math.cos(th0)
y_new = 7.0 * math.sin(th0)
#x_new = x_current*math.cos(theta) - y_current*math.sin(theta)
#y_new = x_current*math.sin(theta) + y_current*math.cos(theta)
z_new = 4.0
rospy.loginfo('the next position is (%s,%s,%s)', x_new, y_new, z_new)
trajectory_point = MultiDOFJointTrajectoryPoint()
trajectory = MultiDOFJointTrajectory()
transform = Transform()
transform.translation.x = x_new
transform.translation.y = y_new
transform.translation.z = z_new
q_rot = quaternion_from_euler(0.0, 0.0, th0 + math.pi / 2.0)
#q_rot = quaternion_multiply(q_rot1, q_orig)
transform.rotation.x = q_rot[0]
transform.rotation.y = q_rot[1]
transform.rotation.z = q_rot[2]
transform.rotation.w = q_rot[3]
trajectory_point.transforms.append(transform)
trajectory.points.append(trajectory_point)
pelican_poses.pose.position.x = x_new
pelican_poses.pose.position.y = y_new
pelican_poses.pose.position.z = z_new
#pelican_poses.pose.orientation = q_new
'''pelican_poses.pose.orientation.x = q_rot[0]
pelican_poses.pose.orientation.y = q_rot[1]
pelican_poses.pose.orientation.z = q_rot[2]
pelican_poses.pose.orientation.w = q_rot[3]'''
pelican_poses.header.stamp = current_time
pelican_poses.header.frame_id = 'world'
path.poses.append(pelican_poses)
pub1.publish(trajectory)
def jointTrajectory2MultiDofTrajectory(self, joint_trajectory):
multi_dof_trajectory = MultiDOFJointTrajectory()
for i in range(0, len(joint_trajectory.points)):
temp_point = MultiDOFJointTrajectoryPoint()
temp_transform = Transform()
temp_transform.translation.x = joint_trajectory.points[i].positions[0]
temp_transform.translation.y = joint_trajectory.points[i].positions[1]
temp_transform.translation.z = joint_trajectory.points[i].positions[2]
temp_transform.rotation.w = math.cos(joint_trajectory.points[i].positions[3]/2.0)
temp_transform.rotation.z = math.sin(joint_trajectory.points[i].positions[3]/2.0)
# Override the rotation
"""
if (i < (len(joint_trajectory.points)-10)):
dx = joint_trajectory.points[i+10].positions[0] - joint_trajectory.points[i].positions[0]
dy = joint_trajectory.points[i+10].positions[1] - joint_trajectory.points[i].positions[1]
yaw = math.atan2(dy, dx)
temp_transform.rotation.z = math.sin(yaw/2)
temp_transform.rotation.w = math.cos(yaw/2)
elif (i < (len(joint_trajectory.points)-1)):
dx = joint_trajectory.points[len(joint_trajectory.points)-1].positions[0] - joint_trajectory.points[i].positions[0]
dy = joint_trajectory.points[len(joint_trajectory.points)-1].positions[1] - joint_trajectory.points[i].positions[1]
yaw = math.atan2(dy, dx)
temp_transform.rotation.z = math.sin(yaw/2)
temp_transform.rotation.w = math.cos(yaw/2)
else:
# same as last one
temp_transform.rotation.z = multi_dof_trajectory.points[i-1].transforms[0].rotation.z
temp_transform.rotation.w = multi_dof_trajectory.points[i-1].transforms[0].rotation.w
"""
temp_vel = Twist()
temp_vel.linear.x = joint_trajectory.points[i].velocities[0]
temp_vel.linear.y = joint_trajectory.points[i].velocities[1]
temp_vel.linear.z = joint_trajectory.points[i].velocities[2]
temp_acc = Twist()
temp_acc.linear.x = joint_trajectory.points[i].accelerations[0]
temp_acc.linear.y = joint_trajectory.points[i].accelerations[1]
temp_acc.linear.z = joint_trajectory.points[i].accelerations[2]
temp_point.transforms.append(temp_transform)
temp_point.velocities.append(temp_vel)
temp_point.accelerations.append(temp_acc)
multi_dof_trajectory.points.append(temp_point)
return multi_dof_trajectory
def odom_callback(odom_msg):
global waypoint_count
global error
global x, y, z
global wpt
x_truth = float(odom_msg.pose.pose.position.x)
y_truth = float(odom_msg.pose.pose.position.y)
z_truth = float(odom_msg.pose.pose.position.z)
wpt = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
wpt.joint_names.append('base_link')
wpt.header = header
if error < 0.1:
print('Waypoint ' + str(waypoint_count) + ' complete')
waypoint_count += 1
if waypoint_count >= np.size(waypoints, axis=0) and error < 0.1:
print('Waypoint list complete!\n'
'Staying at last waypoint: ' + str(x) + ' ' + str(y) + ' ' +
str(z) + '\n')
waypoint_count = 4
x = waypoints[waypoint_count][0]
y = waypoints[waypoint_count][1]
z = waypoints[waypoint_count][2]
theta = waypoints[waypoint_count][3]
error = math.sqrt((x_truth - x)**2 + (y_truth - y)**2 + (z_truth - z)**2)
quat = quaternion.from_euler_angles(0, 0, math.radians(theta))
# quaternion = Quaternion()
transforms = Transform(translation=Point(x, y, z), rotation=quat)
velocities = Twist()
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations], rospy.Time(2))
wpt.points.append(point)
wp_publisher.publish(wpt)