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