def execute_movement(position_sequence, delay_sequence):
if len(position_sequence) != len(delay_sequence):
rospy.logerr("KATANA MOVEMENT ERROR")
jmag_msg = jmag()
for index, position in enumerate(position_sequence):
jmag_msg.header.stamp = rospy.Time.now()
jmag_msg.goal_id.stamp = rospy.Time.now()
jmag_msg.goal.jointGoal.name=joint_names
jmag_msg.goal_id.id = 'Katana Command - ' + str(jmag_msg.header.stamp.secs) + ' . ' + str(jmag_msg.header.stamp.nsecs)
jmag_msg.goal.jointGoal.position=position
#rospy.loginfo(jmag_msg)
#rospy.loginfo("Rate : " + str(1/delay_sequence[index]))
pub.publish(jmag_msg)
rate = rospy.Rate(1.0/delay_sequence[index])
rate.sleep()
def katana_ctrl():
pub = rospy.Publisher('/katana_arm_controller/joint_movement_action/goal', jmag, queue_size=10)
rospy.init_node('katana_arm_our_controler', anonymous=True)
jmag_msg = jmag()
jmag_msg.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
jmag_msg.goal_id.stamp = rospy.Time.now()
jmag_msg.goal_id.id = 'SimpleKatana test - ' + str(jmag_msg.header.stamp.secs) + ' . ' + str(jmag_msg.header.stamp.nsecs)
jmag_msg.goal.jointGoal.name=joint_names
jmag_msg.goal.jointGoal.position = [-1.85,1.57,0.175,-0.35,-1.57,0.175,0.175]
rate = rospy.Rate(0.9)
rate.sleep()
while not rospy.is_shutdown():
jmag_msg.header.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
jmag_msg.goal_id.stamp = rospy.Time.now()
jmag_msg.goal_id.id = 'SimpleKatana test - ' + str(jmag_msg.header.stamp.secs) + ' . ' + str(jmag_msg.header.stamp.nsecs)
jmag_msg.goal.jointGoal.position = [jmag_msg.goal.jointGoal.position[0],jmag_msg.goal.jointGoal.position[1],-jmag_msg.goal.jointGoal.position[2],-jmag_msg.goal.jointGoal.position[3],jmag_msg.goal.jointGoal.position[4],jmag_msg.goal.jointGoal.position[5],jmag_msg.goal.jointGoal.position[6]]
rospy.loginfo(jmag_msg)
pub.publish(jmag_msg)
rate.sleep()
