def talker():
pub = rospy.Publisher('chatter1', JointTrajectory, queue_size=10)
rospy.init_node('talker1', anonymous=True)
rate = rospy.Rate(1) # 10hz
hello_str = JointTrajectory()
hello_str.header.seq = 59
hello_str.header.stamp.secs = 0
hello_str.header.stamp.nsecs = 0
hello_str.header.frame_id = "/workspace"
hello_str.joint_name = [
gripper_r_joint_r, yumi_joint_1_r, yumi_joint_2_r, yumi_joint_3_r,
yumi_joint_4_r, yumi_joint_5_r, yumi_joint_6_r, yumi_joint_7_r
]
# rospy.loginfo(hello_str)
pub.publish(hello_str)
rate.sleep()
def test_controller(self):
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
topic = "/pr2/torso_controller/command"
rospy.init_node('test_pr2_torso_controller',
log_level=rospy.DEBUG,
disable_signals=True)
rospy.loginfo("Preparing to publish on %s" % topic)
ctl = rospy.Publisher(topic, JointTrajectory)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
duration = 0.1
traj = JointTrajectory()
traj.joint_name = "torso_lift_joint"
initialpoint = JointTrajectoryPoint()
initialpoint.positions = 0.0
initialpoint.velocities = 0.0
initialpoint.time_from_start = rospy.Duration(0.0)
finalpoint = JointTrajectoryPoint()
finalpoint.positions = 0.195
finalpoint.velocities = 0.0
finalpoint.time_from_start = rospy.Duration(duration)
# First, move up
traj.points = [initialpoint, finalpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.195)
# Go back to initial position
finalpoint.time_from_start = rospy.Duration(0.0)
initialpoint.time_from_start = rospy.Duration(duration)
traj.points = [finalpoint, initialpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
def test_controller(self):
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
topic = "/pr2/torso_controller/command"
rospy.init_node("test_pr2_torso_controller", log_level=rospy.DEBUG, disable_signals=True)
rospy.loginfo("Preparing to publish on %s" % topic)
ctl = rospy.Publisher(topic, JointTrajectory)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
duration = 0.1
traj = JointTrajectory()
traj.joint_name = "torso_lift_joint"
initialpoint = JointTrajectoryPoint()
initialpoint.positions = 0.0
initialpoint.velocities = 0.0
initialpoint.time_from_start = rospy.Duration(0.0)
finalpoint = JointTrajectoryPoint()
finalpoint.positions = 0.195
finalpoint.velocities = 0.0
finalpoint.time_from_start = rospy.Duration(duration)
# First, move up
traj.points = [initialpoint, finalpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.195)
# Go back to initial position
finalpoint.time_from_start = rospy.Duration(0.0)
initialpoint.time_from_start = rospy.Duration(duration)
traj.points = [finalpoint, initialpoint]
ctl.publish(traj)
time.sleep(duration + 0.1)
self.assertEquals(getjoint("torso_lift_joint"), 0.0)
