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