def functional(): pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.init_node('traj_maker', anonymous=True) time.sleep(4) rate = rospy.Rate(0.01) while not rospy.is_shutdown(): traj_waypoint_1_gary = JointTrajectoryPoint() traj_waypoint_1_rosey = JointTrajectoryPoint() traj_waypoint_1_gary.positions = [0, 0, 0, 0, 0, 0] traj_waypoint_1_rosey.positions = [0, 0, 0, 0, 0, 0] traj_waypoint_1_rosey.time_from_start.secs = 10 traj_waypoint_1_gary.time_from_start.secs = 10 # making message message_gary = FollowJointTrajectoryActionGoal() message_rosey = FollowJointTrajectoryActionGoal() # required headers header_gary = std_msgs.msg.Header(stamp=rospy.Time.now()) header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now()) message_gary.goal.trajectory.header = header_gary message_rosey.goal.trajectory.header = header_rosey message_gary.header = header_gary message_rosey.header = header_rosey # adding in joints joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \ 'joint_5', 'joint_6'] message_gary.goal.trajectory.joint_names = joint_names message_rosey.goal.trajectory.joint_names = joint_names # appending up to 100 points # ex. for i in enumerate(len(waypoints)): append(waypoints[i]) message_gary.goal.trajectory.points.append(traj_waypoint_1_gary) message_rosey.goal.trajectory.points.append(traj_waypoint_1_rosey) # publishing to ROS node pub_gary.publish(message_gary) pub_rosey.publish(message_rosey) rate.sleep() if rospy.is_shutdown(): break break
def publish_msg(self): # Set the message to publish as command. traj_vector = FollowJointTrajectoryActionGoal() # Current ROS time stamp h = Header() h.stamp = rospy.Time.now() traj_vector.header = h traj_vector.goal.trajectory.joint_names.append('arm_1_joint') traj_vector.goal.trajectory.joint_names.append('arm_2_joint') traj_vector.goal.trajectory.joint_names.append('arm_3_joint') traj_vector.goal.trajectory.joint_names.append('arm_4_joint') traj_vector.goal.trajectory.joint_names.append('arm_5_joint') traj_vector.goal.trajectory.joint_names.append('arm_6_joint') traj_vector.goal_id.stamp = h.stamp traj_vector.goal_id.id = 'rosPathPlanner' + str(h.stamp.secs) h2 = Header() h2.stamp.secs = h.stamp.secs + 1 h2.stamp.nsecs = h.stamp.nsecs + 3e8 traj_vector.goal.trajectory.header.stamp = h2.stamp points2pub = [[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]] iter = 0.0 for column in points2pub: point = JointTrajectoryPoint() iter += 1 for q in column: point.positions.append(q) point.time_from_start.nsecs = 0 point.time_from_start.secs = iter * 10 traj_vector.goal.trajectory.points.append(point) print traj_vector print 'All systems go!' self.trajPub.publish(traj_vector)
def callback(self, msg): pub_msg = FollowJointTrajectoryActionGoal() pub_msg.header = msg.header pub_msg.goal_id = msg.goal_id pub_msg.goal.trajectory.header = msg.goal.trajectory.header pub_msg.goal.trajectory.joint_names = [ 'r_thumb_roll', 'r_thumb_pitch', 'r_middle_pitch' ] thumb_r_id = msg.goal.trajectory.joint_names.index('THUMB_R') thumb_p_id = msg.goal.trajectory.joint_names.index('THUMB_P') middle_p_id = msg.goal.trajectory.joint_names.index('MIDDLE_P') pub_msg.goal.trajectory.points = [] for p in msg.goal.trajectory.points: point = JointTrajectoryPoint() point.positions = [ p.positions[thumb_r_id], p.positions[thumb_p_id], p.positions[middle_p_id] ] point.velocities = [ p.velocities[thumb_r_id], p.velocities[thumb_p_id], p.velocities[middle_p_id] ] point.time_from_start = p.time_from_start pub_msg.goal.trajectory.points.append(point) self.joint_command_pub.publish(pub_msg)
def data_loop(self, data): target_angles = eval(data.decode()) trajectory_msg = self.generate_trajectory_msg(target_angles) goal_msg = FollowJointTrajectoryGoal() goal_msg.trajectory = trajectory_msg action_goal_msg = FollowJointTrajectoryActionGoal() action_goal_msg.header = trajectory_msg.header action_goal_msg.goal = goal_msg self.pub.publish(action_goal_msg)
def createRobotMsg(self, msg, start_time, step_time): poruka = FollowJointTrajectoryActionGoal() h = Header() h.stamp = rospy.get_rostime() #print 'Start H' #print h #print 'end H' poruka.header = h poruka.goal.trajectory.joint_names.append('arm_1_joint') poruka.goal.trajectory.joint_names.append('arm_2_joint') poruka.goal.trajectory.joint_names.append('arm_3_joint') poruka.goal.trajectory.joint_names.append('arm_4_joint') poruka.goal.trajectory.joint_names.append('arm_5_joint') poruka.goal.trajectory.joint_names.append('arm_6_joint') poruka.goal_id.id = 'HoCook trajectory ' + str(h.stamp.nsecs) #h.stamp = rospy.get_rostime() h2 = Header() h2.stamp.secs = 0 h2.stamp.nsecs = 0 poruka.goal.trajectory.header.stamp = h2.stamp poruka.header.stamp = h.stamp temp_time = start_time for i in range(0, len(msg.pose_joint_1)): #for i in range(0, 100): tocka = JointTrajectoryPoint() ''' tocka.positions.append(0-1.57) tocka.positions.append(0) tocka.positions.append(1.57) # INVERTIRATI!!!! tocka.positions.append(i*0.5) tocka.positions.append(0) tocka.positions.append(0) ''' tocka.positions.append(msg.pose_joint_1[i]) tocka.positions.append(msg.pose_joint_2[i]) tocka.positions.append(msg.pose_joint_3[i]) # INVERTIRATI!!!! tocka.positions.append(msg.pose_joint_4[i]) tocka.positions.append(msg.pose_joint_5[i]) tocka.positions.append(msg.pose_joint_6[i]) tocka.velocities.append(msg.speed_joint_1[i]) tocka.velocities.append(msg.speed_joint_2[i]) tocka.velocities.append(msg.speed_joint_3[i]) tocka.velocities.append(msg.speed_joint_4[i]) tocka.velocities.append(msg.speed_joint_5[i]) tocka.velocities.append(msg.speed_joint_6[i]) ''' tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) ''' tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) temp_time = temp_time + step_time tocka.time_from_start.nsecs = round( (temp_time - math.floor(temp_time)) * pow(10, 9)) tocka.time_from_start.secs = int(round(math.floor(temp_time))) #print 'time = ', str(temp_time), ', ', str(math.floor(temp_time)), ', ', str(round((temp_time - math.floor(temp_time))*pow(10, 9))), ', ' poruka.goal.trajectory.points.append(tocka) return poruka
def functional(): pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.init_node('traj_maker', anonymous=True) time.sleep(3) rate = rospy.Rate(0.25) commanded_trajectory = [[.31, -.051, .33, -.55, .28, .60], #P1 [.14726, -.014151, .166507, -.33571, .395997, .38657], #P2 [-.09309, .003150, .003559, .16149, .524427, -.1867], #P3 [-.27752, .077886, -.1828, .38563, .682589, -.44665], #P4 [-.09309, .003150, .003559, .16149, .524427, -.1867], #P3 [.14726, -.014151, .166507, -.33571, .395997, .38657]] #P2 #this will repeat while not rospy.is_shutdown(): i=0 for command in commanded_trajectory: # first way to define a point traj_waypoint_1_gary = JointTrajectoryPoint() traj_waypoint_1_rosey = JointTrajectoryPoint # second way to define a point traj_waypoint_gary = JointTrajectoryPoint(positions=command, time_from_start = Duration(1)) traj_waypoint_rosey = JointTrajectoryPoint(positions=command, time_from_start = Duration(1)) # making message message_gary = FollowJointTrajectoryActionGoal() message_rosey = FollowJointTrajectoryActionGoal() # required headers header_gary = std_msgs.msg.Header(stamp=rospy.Time.now()) header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now()) message_gary.goal.trajectory.header = header_gary message_rosey.goal.trajectory.header = header_rosey message_gary.header = header_gary message_rosey.header = header_rosey # adding in joints joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \ 'joint_5', 'joint_6'] message_gary.goal.trajectory.joint_names = joint_names message_rosey.goal.trajectory.joint_names = joint_names # appending up to 100 points # ex. for i in enumerate(len(waypoints)): append(waypoints[i]) if (i > 0): message_gary.goal.trajectory.points.append(old_waypoint) message_gary.goal.trajectory.points.append(traj_waypoint_gary) if (i > 0): message_rosey.goal.trajectory.points.append(old_waypoint) #current location message_rosey.goal.trajectory.points.append(traj_waypoint_rosey) #now "second" for timing # publishing to ROS node #if (i % 3 == 0): # every 3 cycles pub_gary.publish(message_gary) pub_rosey.publish(message_rosey) print 'published: ' + str(commanded_trajectory[i]) rate.sleep() i += 1 old_waypoint = traj_waypoint_rosey old_waypoint.time_from_start.secs = 0 #if i ==1: # rate.sleep() # rate.sleep() # rate.sleep() if i == 5: return if rospy.is_shutdown(): break
from trajectory_msgs.msg import JointTrajectory from trajectory_msgs.msg import JointTrajectoryPoint from control_msgs.msg import FollowJointTrajectoryGoal from std_msgs.msg import Header import random if __name__ == "__main__": pub = rospy.Publisher('/arm_1/arm_controller/follow_joint_trajectory/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.init_node('fake_traj_publisher') rate = rospy.Rate(0.5) # 10 Hz d = rospy.Duration while not rospy.is_shutdown(): fake_traj = FollowJointTrajectoryActionGoal() fake_traj.header = Header() # fake_traj.header.stamp fake_traj.goal = FollowJointTrajectoryGoal() fake_traj.goal.trajectory = JointTrajectory() fake_traj.goal.trajectory.joint_names = [ 'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5' ] # fake_traj.goal.trajectory.points = JointTrajectoryPoint()[] for j in xrange(20): point = JointTrajectoryPoint() point.positions = [random.random() for i in xrange(5)] point.velocities = [random.random() for i in xrange(5)] point.time_from_start = rospy.Duration.from_sec(j)
def functional(commanded_trajectory): pub_gary = rospy.Publisher('/gary/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) pub_rosey = rospy.Publisher('/rosey/joint_trajectory_action/goal', FollowJointTrajectoryActionGoal, queue_size=10) rospy.init_node('traj_maker', anonymous=True) time.sleep(4) rate = rospy.Rate(0.01) i = 0 while not rospy.is_shutdown(): for command in commanded_trajectory: i += 1 # define a point traj_waypoint_1_gary = JointTrajectoryPoint() traj_waypoint_1_rosey = JointTrajectoryPoint() traj_waypoint_1_gary.positions = command traj_waypoint_1_rosey.positions = [-1 * elem for elem in command] traj_waypoint_1_rosey.time_from_start.secs = 0 + 2 * i #first move slow traj_waypoint_1_gary.time_from_start.secs = 0 + 2 * i #debug in terminal print traj_waypoint_1_gary.positions print traj_waypoint_1_rosey.positions # making message message_gary = FollowJointTrajectoryActionGoal() message_rosey = FollowJointTrajectoryActionGoal() # required headers header_gary = std_msgs.msg.Header(stamp=rospy.Time.now()) header_rosey = std_msgs.msg.Header(stamp=rospy.Time.now()) message_gary.goal.trajectory.header = header_gary message_rosey.goal.trajectory.header = header_rosey message_gary.header = header_gary message_rosey.header = header_rosey # adding in joints joint_names = ['joint_1', 'joint_2', 'joint_3', 'joint_4', \ 'joint_5', 'joint_6'] message_gary.goal.trajectory.joint_names = joint_names message_rosey.goal.trajectory.joint_names = joint_names # appending up to 100 points # ex. for i in enumerate(len(waypoints)): append(waypoints[i]) message_gary.goal.trajectory.points = [traj_waypoint_1_gary] message_rosey.goal.trajectory.points = [traj_waypoint_1_rosey] # publishing to ROS node pub_gary.publish(message_gary) pub_rosey.publish(message_rosey) rate.sleep() if rospy.is_shutdown(): break
def moveRobot(self): poruka = FollowJointTrajectoryActionGoal() h = Header() h.stamp = rospy.get_rostime() print 'Start H' print h print 'end H' poruka.header = h poruka.goal.trajectory.joint_names.append('arm_1_joint') poruka.goal.trajectory.joint_names.append('arm_2_joint') poruka.goal.trajectory.joint_names.append('arm_3_joint') poruka.goal.trajectory.joint_names.append('arm_4_joint') poruka.goal.trajectory.joint_names.append('arm_5_joint') poruka.goal.trajectory.joint_names.append('arm_6_joint') poruka.goal_id.id = 'Move trajectory ' + str(h.stamp.nsecs) h.stamp = rospy.get_rostime() poruka.goal.trajectory.header.stamp = h.stamp poruka.header.stamp = h.stamp tocka = JointTrajectoryPoint() ''' tocka.positions.append(0) tocka.positions.append(-1.58) # INVERTIRATI NA CANOPEN tocka.positions.append(1.38005) # -1.38005 INVERTIRATI na IPI!!!! tocka.positions.append(0) tocka.positions.append(1.36365) # -1.36365 # INVERTIRATI NA CANOPEN tocka.positions.append(0) ''' if (1 == 1): tocka.positions.append(0) tocka.positions.append(0) tocka.positions.append(-1.57) # INVERTIRATI!!!! tocka.positions.append(0) tocka.positions.append(0) tocka.positions.append(0) else: tocka.positions.append(0) tocka.positions.append(0.0) tocka.positions.append(1.636) # INVERTIRATI!!!! tocka.positions.append(0.0) tocka.positions.append(-0.0637) tocka.positions.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.velocities.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.accelerations.append(0) tocka.time_from_start.nsecs = 0 * pow(10, 9) tocka.time_from_start.secs = 7 poruka.goal.trajectory.points.append(tocka) self.trajPub_red.publish(poruka) #self.trajPub_blue.publish(poruka) print "" print poruka print rospy.get_rostime() print h.stamp