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