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 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 __init__(self):
self.traj_pub = rospy.Publisher('/bebop/command/trajectory', MultiDOFJointTrajectory,queue_size=1)
self.pose_sub = rospy.Subscriber("/bebop/command/control", Pose,self.pose_callback)
self.traj = MultiDOFJointTrajectory()
self.r = rospy.Rate(60)
self.pose = Pose()
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 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)
multi_dof_trajectory.points.append(temp_point)
return multi_dof_trajectory
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.hz = 50
self.dt = 1.0/self.hz
total_time = 30
self.length = total_time*self.hz
self.header = std_msgs.msg.Header()
self.quaternion = tf.transformations.quaternion_from_euler(0,0,0)
self.velocities = Twist()
self.accelerations = Twist()
self.traj_pub = rospy.Publisher('/command/trajectory', MultiDOFJointTrajectory,queue_size=1)
self.msg = MultiDOFJointTrajectory()
self.msg.points = [0]*self.length
self.msg.header = self.header
self.speed_mod1 = .6
self.speed_mod2 = .75
self.speed_mod3 = .95
self.mag = 1
#bunch of stuff
self.t_o = 0
self.t_1 = 5+ self.t_o
self.t_2 = self.t_1 + 2.0*math.pi/self.speed_mod1
self.t_3 = self.t_2 + 2.0*math.pi/self.speed_mod2
self.t_4 = self.t_3 + 2.0*math.pi/self.speed_mod3
self.t_rest = self.t_4 +3.0
self.t_5 = self.t_rest + 2.0*math.pi/self.speed_mod1
self.t_6 = self.t_5 + 2.0*math.pi/self.speed_mod2
self.t_7 = self.t_6 + 2.0*math.pi/self.speed_mod3
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 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 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 __init__(self):
# Node(s)
rospy.init_node('waypoint_publisher')
# Var(s)
self.kNanoSecondsInSecond = 1000000000
self.new_room = 1
self.wait_flag = 1
self.front = 0
self.right = 0
self.left = 0
self.back = 0
self.current_odom = Odometry()
self.current_scan = LaserScan()
self.trajectory = MultiDOFJointTrajectory()
# Publisher(s)
# self.waypoint_pub = rospy.Publisher('/firefly/command/trajectory', MultiDOFJointTrajectory, queue_size = 10)
self.waypoint_pub = rospy.Publisher(
'/firefly/command/calculated_trajectory',
MultiDOFJointTrajectory,
queue_size=10)
# Function(s)
self.waypoint_calculator()
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 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 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 __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 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 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 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 splitTrajectory(self, traj):
start_point = int(self.split_start * self.rate)
end_point = int(self.split_end * self.rate)
self.trajectory = MultiDOFJointTrajectory()
for i in range(start_point, end_point + 1):
self.trajectory.points.append(traj.points[i])
self.trajectory.points[
i - start_point].time_from_start = rospy.Duration(
float(i - start_point) / float(self.rate))
self.endless_trajectory = copy.deepcopy(self.trajectory)
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 __init__(self, frequency, future_length):
self.hz = frequency
self.dt = 1.0 / frequency
self.future_length = future_length
self.header = std_msgs.msg.Header()
self.quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
self.velocities = Twist()
self.accelerations = Twist()
self.traj_pub = rospy.Publisher('/command/trajectory',
MultiDOFJointTrajectory,
queue_size=10)
self.msg = MultiDOFJointTrajectory()
self.msg.points = [0] * self.future_length
self.msg.header = self.header
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)
def __init__(self):
self.trajectory_point_pub = rospy.Publisher(
'trajectory_point_ref', MultiDOFJointTrajectoryPoint, queue_size=1)
rospy.Subscriber('multi_dof_trajectory',
MultiDOFJointTrajectory,
self.multi_dof_trajectory_cb,
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 = MultiDOFJointTrajectory()
self.current_trajectory_point = MultiDOFJointTrajectoryPoint()
def command_pose(self, position, orientation):
msg_traj = MultiDOFJointTrajectory()
msg_traj.header.stamp = rospy.Time()
msg_traj.joint_names = ["base_link"]
p = Vector3(position[0], position[1], position[2])
q = Quaternion(
orientation[0], orientation[1], orientation[2], orientation[3]
)
transforms = [Transform(translation=p, rotation=q)]
point = MultiDOFJointTrajectoryPoint(
transforms, [Twist()], [Twist()], rospy.Time()
)
msg_traj.points.append(point)
self.pub_traj.publish(msg_traj)
def handle_execute_request(self, req):
if "manipulator" in req.REQUEST_TYPE:
self.group = moveit_commander.MoveGroupCommander("manipulator")
elif "gripper" in req.REQUEST_TYPE:
self.group = moveit_commander.MoveGroupCommander("gripper")
else:
rospy.logwarn("PlanningService::handle_plan_request() -- requested group is not recognized")
return False
#build moveit msg for display
joint_multi_traj_msg = MultiDOFJointTrajectory()
robot_traj_msg = RobotTrajectory()
robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY
robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg
self.group.execute(robot_traj_msg, wait=True)
def handle_show_request(self, req):
display_trajectory = DisplayTrajectory()
display_trajectory.trajectory_start = self.robot.get_current_state()
#build moveit msg for display
joint_multi_traj_msg = MultiDOFJointTrajectory()
robot_traj_msg = RobotTrajectory()
robot_traj_msg.joint_trajectory = req.JOINT_TRAJECTORY
robot_traj_msg.multi_dof_joint_trajectory = joint_multi_traj_msg
display_trajectory.trajectory.append(robot_traj_msg)
rospy.loginfo("PlanningService::handle_show_request() -- showing trajectory %s", req.JOINT_TRAJECTORY)
self.display_trajectory_publisher.publish(display_trajectory);
return True
def getPointReference(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)) command.points[0].transforms[0].translation.x = self.pointreference.position.x command.points[0].transforms[0].translation.y = self.pointreference.position.y return command
def callback_odom(self, msg_odom):
self.pose_x = msg_odom.pose.pose.position.x
self.pose_y = msg_odom.pose.pose.position.y
self.pose_z = msg_odom.pose.pose.position.z
self.got_odom = True
if self.restarting:
self.astar = Point(0, 0, 1.5)
self.dnn = Point(0, 0, 1.5)
if abs(self.pose_x - 0) < 0.01:
self.got_odom = False
self.got_astar = False
self.got_dnn = False
print('Returned to start position')
print('Waiting to reset Astar and DNN')
rospy.sleep(3)
self.restarting = False
wpt = MultiDOFJointTrajectory()
header = Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
wpt.joint_names.append('base_link')
wpt.header = header
if self.namespace == "DJI":
direction_theta = -45
else:
direction_theta = 0
quat = quaternion.from_euler_angles(0, 0,
math.radians(direction_theta))
transforms = Transform(translation=Point(0, 0, 1.5), rotation=quat)
velocities = Twist()
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transforms],
[velocities], [accelerations],
rospy.Time(2))
wpt.points.append(point)
self.waypoint_pub.publish(wpt)
if abs(self.pose_x - 5) < 0.1 and self.training:
self.restarting = True
print('Made it to goal location')