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 execute_cb(self, goal): success = True #create action for arm arm_action = FollowJointTrajectoryAction() #create two seperate goals, one for base and one for arm base_fjtag = FollowJointTrajectoryActionGoal() arm_fjtag = FollowJointTrajectoryActionGoal() #set the base goal and name the virtual joints base_goal = FollowJointTrajectoryGoal() base_jt = JointTrajectory() base_jt.joint_names = ["virtual_x", "virtual_y", "virtual_theta"] #set the arm goal and name the arm joints arm_goal = FollowJointTrajectoryGoal() arm_jt = JointTrajectory() arm_jt.joint_names = [ "arm_joint_1", "arm_joint_2", "arm_joint_3", "arm_joint_4", "arm_joint_5" ] for pt in goal.trajectory.points: # Fetch the base position from Moveit plan pt_temp = JointTrajectoryPoint() pt_temp.positions.append(pt.positions[6]) pt_temp.positions.append(pt.positions[7]) pt_temp.positions.append(pt.positions[5]) pt_temp.time_from_start = pt.time_from_start base_jt.points.append(pt_temp) pt_temp = JointTrajectoryPoint() #allocate the last five pt.position(for arm) elements to arm trajectory pt_temp.positions = pt.positions[0:5] # pt_temp.velocities = pt.velocities[0:5] # pt_temp.accelerations = pt.accelerations[0:5] # pt_temp.time_from_start = pt.time_from_start + rospy.Duration(0.1) arm_jt.points.append(pt_temp) base_goal.trajectory = base_jt arm_goal.trajectory = arm_jt base_fjtag.goal = base_goal arm_fjtag.goal = arm_goal arm_action = arm_goal # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo("This action has been preempted") self._as.set_preempted() success = False self._as.set_cancelled() #if preempted, can do something here else: self.pub1.publish(base_fjtag) self.arm_client.send_goal_and_wait(arm_action) success = True # self.pub2.publish(arm_fjtag) if success: rospy.loginfo('Action Succeeded') self._as.set_succeeded()
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) fake_traj.goal.trajectory.points.append(point) # fake_traj.goal.trajectory.points.positions = [[random.random() ]
def execute_cb(self, goal): success = True #create action for arm arm_action = FollowJointTrajectoryAction() #create two seperate goals, one for base and one for arm base_fjtag = FollowJointTrajectoryActionGoal() arm_fjtag = FollowJointTrajectoryActionGoal() #set the base goal and name the virtual joints base_goal = FollowJointTrajectoryGoal() base_jt = JointTrajectory() base_jt.joint_names = ["virtual_x", "virtual_y", "virtual_theta"] #set the arm goal and name the arm joints arm_goal = FollowJointTrajectoryGoal() arm_jt = JointTrajectory() arm_jt.joint_names = ["arm_joint_1", "arm_joint_2", "arm_joint_3","arm_joint_4","arm_joint_5"] for pt in goal.trajectory.points: # Fetch the base position from Moveit plan pt_temp = JointTrajectoryPoint() pt_temp.positions.append(pt.positions[6]) pt_temp.positions.append(pt.positions[7]) pt_temp.positions.append(pt.positions[5]) pt_temp.time_from_start = pt.time_from_start base_jt.points.append(pt_temp) pt_temp = JointTrajectoryPoint() #allocate the last five pt.position(for arm) elements to arm trajectory pt_temp.positions = pt.positions[0:5] # pt_temp.velocities = pt.velocities[0:5] # pt_temp.accelerations = pt.accelerations[0:5] # pt_temp.time_from_start = pt.time_from_start + rospy.Duration(0.1) arm_jt.points.append(pt_temp) base_goal.trajectory = base_jt arm_goal.trajectory = arm_jt base_fjtag.goal = base_goal arm_fjtag.goal = arm_goal arm_action = arm_goal # check that preempt has not been requested by the client if self._as.is_preempt_requested(): rospy.loginfo("This action has been preempted") self._as.set_preempted() success = False self._as.set_cancelled() #if preempted, can do something here else: self.pub1.publish(base_fjtag) self.arm_client.send_goal_and_wait(arm_action) success = True # self.pub2.publish(arm_fjtag) if success: rospy.loginfo('Action Succeeded') self._as.set_succeeded()