def update_setpoint(self):
#t = rospy.Time.now().to_sec()
#self.vel = step(t)
try:
v = jmu.list_of_xyz(self.twist.linear)[0]
x = jmu.list_of_xyz(self.pose.position)[0]
except AttributeError:
print 'no pose'
else:
if self.phase == 0:
if x > self.xlim:
self.vel = -self.vlim
self.phase = 1
else:
if x < -self.xlim:
self.vel = self.vlim
self.phase = 0
def gazebo_truth_callback(self, msg):
self.truth_pos.append(jmu.list_of_xyz(msg.pose.pose.position))
self.truth_ori.append(jmu.list_of_xyzw(msg.pose.pose.orientation))
self.truth_lvel.append(jmu.list_of_xyz(msg.twist.twist.linear))
self.truth_rvel.append(jmu.list_of_xyz(msg.twist.twist.angular))
self.truth_stamp.append(msg.header.stamp.to_sec())
def smocap_cbk(self, msg):
self.mocap_pos.append(jmu.list_of_xyz(msg.pose.pose.position))
self.mocap_ori.append(jmu.list_of_xyzw(msg.pose.pose.orientation))
self.mocap_stamp.append(msg.header.stamp)