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 publish_command(position,velocity):
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,'/firefly4/command/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 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 = 'hummingbird/base_link'
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 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 = 'frame'
traj.header = header
traj.joint_names = 'nothing'
transforms = Transform(translation=Point(-5.0, 4, 2),
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(1, 0, 0))
point = MultiDOFJointTrajectoryPoint([transforms], [velocities],
[accelerations],
rospy.Duration(self.RHP_time))
traj.points.append(point)
self.uav_traj_pub.publish(traj)
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 __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 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 publish_trajectory(self, trajectory):
trajectory_msg = MultiDOFJointTrajectory()
trajectory_msg.header.frame_id = 'world'
trajectory_msg.joint_names = ['base']
for idx in range(len(trajectory)):
point = trajectory[idx]
point['time'] = trajectory[idx]['time']
transform = Transform()
transform.translation = Vector3(*(point['point'].tolist()))
transform.rotation = Quaternion(
*(vec_to_quat(point['velocity']).tolist()))
velocity = Twist()
velocity.linear = Vector3(*(point['velocity'].tolist()))
acceleration = Twist()
acceleration.linear = Vector3(*(point['acceleration'].tolist()))
trajectory_msg.points.append(
MultiDOFJointTrajectoryPoint([transform], [velocity],
[acceleration],
rospy.Duration(point['time'])))
trajectory_msg.header.stamp = rospy.Time.now()
self.traj_pub.publish(trajectory_msg)
def random_selection(self):
if self.got_dnn and self.got_astar and self.restarting is False: \
# Choose every n times
if self.choice_current == self.choice_count:
choices = ['astar', 'dnn']
prob = [1 - self.dagger_beta, self.dagger_beta]
self.active = np.random.choice(choices, p=prob)
self.choice_current = 1
else:
self.choice_current += 1
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))
velocities = Twist()
accelerations = Twist()
point = MultiDOFJointTrajectoryPoint([transforms],
[velocities], [accelerations],
rospy.Time(2))
wpt.points.append(point)
self.waypoint_pub.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 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 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 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 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 main(x, y, z, a, b, c):
rospy.init_node('init1', anonymous=True)
now = rospy.get_rostime()
rospy.loginfo("Current time %i %i", now.secs, now.nsecs)
pub1 = rospy.Publisher('/firefly/command/trajectory',
MultiDOFJointTrajectory,
queue_size=100)
# initial position of the first pelican
trajectory1 = MultiDOFJointTrajectory()
trajectory1_point = MultiDOFJointTrajectoryPoint()
trajectory1.header.frame_id = 'firefly/base_link'
transform1 = Transform()
#transform1.translation.x = 0
#transform1.translation.y = 0
#transform1.translation.z = 4
transform1.translation = Vector3(x, y, z)
q_rot = quaternion_from_euler(a, b, c)
transform1.rotation.x = q_rot[0]
transform1.rotation.y = q_rot[1]
transform1.rotation.z = q_rot[2]
transform1.rotation.w = q_rot[3]
velocity1 = Twist()
accelerate1 = Twist()
t = rospy.Time.now()
trajectory1.header.stamp.secs = t.secs
trajectory1.header.stamp.nsecs = t.nsecs
trajectory1_point.transforms.append(transform1)
trajectory1_point.velocities.append(velocity1)
trajectory1_point.accelerations.append(accelerate1)
trajectory1.points.append(trajectory1_point)
# publish the related messages
rate = rospy.Rate(100)
while not rospy.is_shutdown():
pub1.publish(trajectory1)
rate.sleep()
def multiple_points(points, v_max, a_max, pub):
"""
Function that publishes trajectory of multiple points to publisher pub.
args:
points: list of tuples (x, y, z) -- (multiple points that we want to add to trajectory)
v_max: float -- maximal wanted speed
a_max: float -- maximal wanted acceleration
pub: Publisher to which you want to publish your trajectory
"""
traj_2_points = MultiDOFJointTrajectory()
for i, point in enumerate(points):
try:
traj_2_points.points.extend(
trajectory_two_points(point, points[i + 1], v_max, a_max,
pub).points)
except IndexError:
pass
def getCarrotTrajectory(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_carrot_value): self.new_carrot_value = False self.current_pose_reference.position.x = self.current_pose.position.x + self.carrot_x self.current_pose_reference.position.y = self.current_pose.position.y + self.carrot_y 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 return command
def __init__(self, x_o, y_o, z_o, frequency, future_length):
self.x_o = x_o
self.y_o = y_o
self.z_0 = z_o
self.t = 0
self.hz = frequency
self.dt = 1.0 / frequency
self.length = future_length
#self.msg = MultiDOFJointTrajectory()
self.header = std_msgs.msg.Header()
self.header.frame_id = 'frame'
self.quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
self.msg = MultiDOFJointTrajectory()
self.msg.points = [0] * self.length
#transforms = Transform(translation=Point(x,y,z), rotation=Quaternion(self.quaternion[0],self.quaternion[1],self.quaternion[2],self.quaternion[3]))
self.velocities = Twist()
self.accelerations = Twist()
# p = MultiDOFJointTrajectoryPoint([transforms], [self.velocities], [self.accelerations], rospy.Time(i*self.dt))
self.count = 0
def send_waypoint(self, x, y, z, yaw, pitch, roll):
trajectory_msg = MultiDOFJointTrajectory()
trajectory_msg.header.stamp = rospy.Time.now()
desired_position = Vector3()
desired_position.x = x
desired_position.y = y
desired_position.z = z
desired_rotation = Quaternion(
*tf_conversions.transformations.quaternion_from_euler(
roll, pitch, yaw))
transform = Transform()
transform.translation = desired_position
transform.rotation = desired_rotation
point = MultiDOFJointTrajectoryPoint()
point.transforms.append(transform)
trajectory_msg.points.append(point)
self.trajectory_pub.publish(trajectory_msg)
rospy.loginfo("Published waypoint")
def __init__(self):
self.x_o = 0
self.y_o = 0
self.z_o = 0
self.t = 0
self.roll_o = 0
self.pitch_o = 0
self.yaw_o = 0
self.quaternion = [0, 0, 0, 0]
self.future_length = 20
self.dt = 0.1
self.ref_msg = PoseStamped()
self.msg = MultiDOFJointTrajectory()
self.header = std_msgs.msg.Header()
self.quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
self.traj_pub = rospy.Publisher('/command/trajectory',
MultiDOFJointTrajectory,
queue_size=10)
self.ref_pub = rospy.Publisher('/target/position',
PoseStamped,
queue_size=10)
self.msg.points = [0] * self.future_length
self.print_flag = 0
self.subscribe_topic_name = rospy.get_param('~SubscribeTopic',
"/ardu/odom")
self.subscribe = rospy.Subscriber(self.subscribe_topic_name, Odometry,
self.callback)
self.first_run_flag = 0
self.publish_oscilate_flag = 0
self.finish_flag = 0
self.rest_flag = 0
self.mag = .5
def __init__(self, carrot_v, carrot_w, sim, rate, trajectory_rate): self.sim = sim self.rate = rate self.trajectory_rate = trajectory_rate self.trajectory_length = 0 self.commander_mode = 0 self.trajectory_reference = MultiDOFJointTrajectory() self.current_pose_reference = Pose() self.startFlag = False self.new_carrot_value = False self.carrot_v = carrot_v; self.carrot_w = carrot_w; self.carrot_x = 0.0 self.carrot_y = 0.0 self.new_trajectory = False self.current_pose = Pose() self.current_twist = Twist() self.trajectory_index = 0 self.pointreference = Pose() self.w = 0 self.v = 0 self.setMode(0)
def __init__(self):
self.x_o = 0
self.y_o = 0
self.z_o = 0
self.t = 0
self.roll_o = 0
self.pitch_o = 0
self.yaw_o = 0
self.quaternion = [0, 0, 0, 0]
self.future_length = 20
self.dt = 0.1
self.ref_msg = PoseStamped()
self.msg = MultiDOFJointTrajectory()
self.header = std_msgs.msg.Header()
self.quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
self.traj_pub = rospy.Publisher('/command/trajectory',
MultiDOFJointTrajectory,
queue_size=10)
self.ref_pub = rospy.Publisher('/target/position',
PoseStamped,
queue_size=10)
self.msg.points = [0] * self.future_length
self.print_flag = 0
self.first_run_flag = 0
self.publish_oscilate_flag = 0
self.finish_flag = 0
self.rest_flag = 0
rospy.Timer(rospy.Duration(.02), self.callback)
self.mag = .5
self.speed = 1.2
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()
velocities.linear.x = 100
velocities.linear.y = 0
velocities.linear.z = 0
velocities.angular.x = 0
velocities.angular.y = 0
velocities.angular.z = 0
accelerations=Twist()
accelerations.linear.x = 100
accelerations.linear.y = 0
accelerations.linear.z = 0
accelerations.angular.x = 0
accelerations.angular.y = 0
accelerations.angular.z = 0
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 setMode(self, mode):
self.commander_mode = mode
if (self.commander_mode == 0):
print("Entering MANUAL mode.")
self.w = 0
self.v = 0
elif (self.commander_mode == 1):
print("Entering CARROT mode.")
self.current_pose_reference.position.x = self.current_pose.position.x
self.current_pose_reference.position.y = self.current_pose.position.y
elif (self.commander_mode == 2):
print("Entering TRAJECTORY mode.")
self.current_pose_reference.position.x = self.current_pose.position.x
self.current_pose_reference.position.y = self.current_pose.position.y
self.current_pose_reference.orientation.x = self.current_pose.orientation.x
self.current_pose_reference.orientation.y = self.current_pose.orientation.y
self.current_pose_reference.orientation.z = self.current_pose.orientation.z
self.current_pose_reference.orientation.w = self.current_pose.orientation.w
self.trajectory_reference = MultiDOFJointTrajectory()
elif (self.commander_mode == 3):
print("Entering POINT mode.")
self.pointreference.position.x = self.current_pose.position.x
self.pointreference.position.y = self.current_pose.position.y
def traj_to_msg(traj):
"""Converts the trajectory planned by the planner to a publishable ROS message
Args:
traj (gennav.utils.Trajectory): Trajectory planned by the planner
Returns:
geometry_msgs/MultiDOFJointTrajectory.msg: A publishable ROS message
"""
traj_msg = MultiDOFJointTrajectory(points=[],
joint_names=None,
header=None)
for state, timestamp in zip(traj.path, traj.timestamps):
quaternion = tf.transformations.quaternion_from_euler(
state.orientation.roll,
state.orientation.pitch,
state.orientation.yaw,
)
velocity = Twist()
acceleration = Twist()
transforms = Transform(
translation=Point(state.position.x, state.position.y,
state.position.z),
rotation=Quaternion(quaternion[0], quaternion[1], quaternion[2],
quaternion[3]),
)
traj_point = MultiDOFJointTrajectoryPoint(
transforms=[transforms],
velocities=[velocity],
accelerations=[acceleration],
time_from_start=timestamp,
)
traj_msg.points.append(traj_point)
return traj_msg
def ref_sub(self, msg):
traj = MultiDOFJointTrajectory()
traj.points = []
traj.points.append(msg)
if not self.status == "OFF":
self.traj_pub.publish(traj)
def trajectory_in_camera_fov(self, data):
"""generates trajectory in camera workspace"""
#self.end_point = self.end_point + np.array([-self.target_velocity*0.05,0, 0])
odom_msg = Odometry()
odom_msg.pose.pose.position.x = self.end_point[0]
odom_msg.pose.pose.position.y = self.end_point[1]
odom_msg.pose.pose.position.z = self.end_point[2]
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = 'vicon'
self.target_odom.publish(odom_msg)
start = time.time()
start_point = np.zeros((0,3))
if self.traj_gen_counter != 0:
start_point = np.concatenate((start_point, self.p_eoe), 0) # was self.p_eoe
else:
start_point = np.concatenate((start_point, self.Pglobal[np.newaxis]), 0) # new axis was needed because you dont maintain uniformity
points_in_cone = self.generate_regular_pyramid_grid()
occupied_points = ros_numpy.numpify(data)
#for k in range(len(occupied_points)):
# f7 = open('point_cloud.txt', 'a')
# f7.write('%s, %s, %s, %s, %s, %s\n' %(self.traj_gen_counter, self.RHP_time, len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2]))
#if self.traj_gen_counter == 0:
## for k in range(len(occupied_points)):
# f7 = open('point_cloud.txt', 'a')
# f7.write('%s, %s, %s, %s\n' %(len(occupied_points), occupied_points[k][0], occupied_points[k][1], occupied_points[k][2]))
if len(occupied_points) != 0:
points_in_cone = self.generate_final_grid(points_in_cone, occupied_points)
"""
points_in_cone = [x[np.newaxis] for x in points_in_cone]
pp1 = [np.dot(self.Rcdoc_cdl[0:3, 0:3].T, x.T) for x in points_in_cone]
# now translate from visensor frame to cg frame
pp2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in pp1]
# now rotate the cloud in world frame
points_in_cone = [self.Pglobal + np.dot(self.Rglobal.T, x).ravel() for x in pp2]
# remove z points if they are going inside the ground, assuming ground is z = 0
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
"""
#points_in_cone = np.concatenate((start_point, points_in_cone), 0)
p1 = [np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
#p1 = [self.Rvibase_cl[0:3, 3:4] + np.dot(self.Rcdoc_cdl[0:3,0:3], x[np.newaxis].T) for x in points_in_cone]
p2 = [self.Rcg_vibase[0:3, 3:4] + np.dot(self.Rcg_vibase[0:3, 0:3].T, x) for x in p1]
points_in_cone = [self.Pglobal + np.dot(self.Rglobal, x).ravel() for x in p2]
points_in_cone = [x for x in points_in_cone if not x[2] <= 0]
points_in_cone = np.concatenate((start_point, points_in_cone), 0)
# publish generated pointcloud
header = std_msgs.msg.Header()
header.stamp = rospy.Time.now()
header.frame_id = 'world'
p = pc2.create_cloud_xyz32(header, points_in_cone)
self.pcl_pub.publish(p)
kdt_points_in_cone = cKDTree(points_in_cone)
closest_to_end = kdt_points_in_cone.query(self.end_point, 1)
#closest_to_end_index = points_in_cone[closest_to_end[1]]
#closest_to_end_point = (closest_to_end_index[0], closest_to_end_index[1], closest_to_end_index[2])
end_point_index = closest_to_end[1]
#one dimension simplicial complex which is basically a graph
rips = gudhi.RipsComplex(points = points_in_cone, max_edge_length = 1.5*self.resolution)
f = rips.create_simplex_tree(max_dimension = 1)
# make graph
G = nx.Graph()
G.add_nodes_from(range(f.num_vertices()))
edge_list = [(simplex[0][0], simplex[0][1], {'weight': simplex[1]}) if len(simplex[0])==2 else None for simplex in f.get_skeleton(1)]
edge_list = [k for k in edge_list if k is not None]
G.add_edges_from(edge_list)
try:
shortest_path = nx.shortest_path(G, source = 0, target = end_point_index, weight = 'weight', method = 'dijkstra')
path = np.array([points_in_cone[j] for j in shortest_path])
except:
print 'No path between start and end'
#f1 = open('path.txt', 'a')
#f1.write('%s, %s\n' %(path, points_in_cone[end_point_index]))
#length = nx.shortest_path_length(G, source = 0, target = end_point_index, weight = 'weight', method='dijkstra')
# planning horizon is a fixed (5 for now) segments, trajectory will be planned only for these segments,
# execution horizon will be just 3 segments
# at current resolution of 0.25m, that means a trajecotory of approximately 1.25m will be planned and 0.75m will be executed
# at each time the quad plans its motion, depending on how fast it can replan, these values would be changed
no_of_segments = 4
no_of_segments_to_track = 1
path = path[:no_of_segments+1] # n segments means n+1 points in the path
path = zip(*path)
# now construct the minimum snap trajectory on the minimum path
waypoint_specified = True
waypoint_bc = False
planning_counter = 2
T, p1, p2, p3 = Minimum_snap_trajetory(planning_counter, self.uav_velocity, path, waypoint_specified, waypoint_bc, self.v_eoe, self.a_eoe, self.j_eoe).construct_polynomial_trajectory()
#self.plot_graph_and_trajectory(points_in_cone, occupied_points, G, path, T, p1, p2, p3)
N = 8
p1 = [p1[i:i + N] for i in range(0, len(p1), N)]
[i.reverse() for i in p1]
p2 = [p2[i:i + N] for i in range(0, len(p2), N)]
[i.reverse() for i in p2]
p3 = [p3[i:i + N] for i in range(0, len(p3), N)]
[i.reverse() for i in p3]
t = []; xx = []; yy = []; zz = []
vx = []; vy = []; vz = []; accx = []; accy = []; accz = []
jx = []; jy = []; jz = []
#print T
traj_frequency = 100
for ii in range(no_of_segments_to_track):
#t.append(np.linspace(T[ii], T[ii+1], int((T[ii+1]-T[ii])*traj_frequency)))
t.append(np.linspace(T[ii], T[ii+1], 21))
xx.append(np.poly1d(p1[ii]))
vx.append(np.polyder(xx[-1], 1)); accx.append(np.polyder(xx[-1], 2))
jx.append(np.polyder(xx[-1], 3))#; sx.append(np.polyder(xx[-1], 4))
yy.append(np.poly1d(p2[ii]))
vy.append(np.polyder(yy[-1], 1)); accy.append(np.polyder(yy[-1], 2))
jy.append(np.polyder(yy[-1], 3))#; sy.append(np.polyder(yy[-1], 4))
zz.append(np.poly1d(p3[ii]))
vz.append(np.polyder(zz[-1], 1)); accz.append(np.polyder(zz[-1], 2))
jz.append(np.polyder(zz[-1], 3))#; sz.append(np.polyder(zz[-1], 4))
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' # testing for now
trajectory_start_time = data.header.stamp.to_sec()#self.RHP_time
for i in range(no_of_segments_to_track): # "changed to no_of_segments_to_track" instead of "no_of_segments"
for j in t[i]:
self.RHP_time = j + trajectory_start_time
xdes = xx[i](j); ydes = yy[i](j); zdes = zz[i](j)
vxdes = vx[i](j); vydes = vy[i](j); vzdes = vz[i](j)
axdes = accx[i](j); aydes = accy[i](j); azdes = accz[i](j)
jxdes = jx[i](j); jydes = jy[i](j); jzdes = jz[i](j)
# for now angular acceleration Point msg of MultiDOFJointTrajectory() msg is used to specify the desired direction
#vector = np.array([self.end_point[0]-xdes, self.end_point[1]-ydes, self.end_point[2]-zdes])
#vector = np.array([self.p_eoe[0][0]-xdes, self.p_eoe[0][1]-ydes, self.p_eoe[0][2]-zdes])
vector = np.array([self.end_point[0]-self.Pglobal[0], self.end_point[1]-self.Pglobal[1], self.end_point[2]-self.Pglobal[2]])
#vector = np.array([self.Vglobal[0], self.Vglobal[1], self.Vglobal[2]])
direction = vector/np.linalg.norm(vector)
transforms = Transform(translation = Point(xdes, ydes, zdes), rotation = Quaternion(0,0,0,1))
velocities = Twist(linear = Point(vxdes, vydes, vzdes), angular = Point(0,0,0))
accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(direction[0],direction[1],direction[2]))
#accelerations = Twist(linear = Point(axdes, aydes, azdes), angular = Point(1,0,0))
point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Duration(self.RHP_time))
traj.points.append(point)
#self.uav_traj_pub.publish(traj)
#traj.points.pop(0)
self.p_eoe = np.array([[xdes, ydes, zdes]])
self.v_eoe = np.array([[vxdes, vydes, vzdes]])
self.a_eoe = np.array([[axdes, aydes, azdes]])
self.j_eoe = np.array([[jxdes, jydes, jzdes]])
self.uav_traj_pub.publish(traj)
time_taken = time.time()-start
self.traj_gen_counter += 1
print 'total time taken to execute the callbacak is:', time_taken
target_linear_vel = 0.0
control_linear_vel = 0.0
target_angular_vel = 0.0
control_angular_vel = 0.0
ang_z = 0.0
print(vels(target_linear_vel, target_angular_vel))
else:
if (key == '\x03'):
break
if status == 20:
print(msg)
status = 0
# 创建MultiDOFJointTrajectory消息
trajectory_msg = MultiDOFJointTrajectory()
trajectory_msg.joint_names.append("base_link")
trajectory_points = MultiDOFJointTrajectoryPoint()
#创建transform消息
tran = Transform()
control_linear_vel = makeSimpleProfile(
control_linear_vel, target_linear_vel,
(LIN_VEL_STEP_SIZE / 2.0)) # 限速
control_angular_vel = makeSimpleProfile(
control_angular_vel, target_angular_vel,
(ANG_VEL_STEP_SIZE / 2.0)) # 限速
pos_x += control_linear_vel
pos_y += control_angular_vel
tran.translation.z = 1
tran.translation.x = pos_x
import rospy
import math
import time
firefly_command_publisher = rospy.Publisher('/firefly/command/trajectory', MultiDOFJointTrajectory, queue_size=10)
desired_yaw_to_go_degree=-10
desired_x_to_go=2
desired_y_to_go=2.5
desired_z_to_go=2.5
quaternion = tf.transformations.quaternion_from_euler(0, 0, math.radians(desired_yaw_to_go_degree))
traj = MultiDOFJointTrajectory()
header = std_msgs.msg.Header()
header.stamp = rospy.Time()
header.frame_id = 'frame'
traj.joint_names.append('base_link')
traj.header=header
transforms =Transform(translation=Point(desired_x_to_go, desired_y_to_go, desired_z_to_go), rotation=Quaternion(quaternion[0],quaternion[1],quaternion[2],quaternion[3]))
velocities =Twist()
accelerations=Twist()
point = MultiDOFJointTrajectoryPoint([transforms],[velocities],[accelerations],rospy.Time(2))
traj.points.append(point)
