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