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)