def __init__(self, a_name): """ Constructor :param a_name: """ super(Object, self).__init__(time_out=0.1) # Set duration of Watchdog expiry self._name = '' self._state = ObjectState() self._cmd = ObjectCmd() self._pub = None self._sub = None self.pub_flag = True self._active = False self._pose_cmd_set = False # Flag to check if a Pose command has been set from the Object self._wrench_cmd_set = False # Flag to check if a Wrench command has been set from the Object
import rospy import numpy as np import sys from ambf_msgs.msg import ObjectState, WorldState, WorldCmd from matplotlib import pyplot import datetime wCmd = WorldCmd() wCmd.enable_step_throttling = True wCmd.step_clock = True wCmd.n_skip_steps = 1 g1State = ObjectState() g2State = ObjectState() wState = WorldState() global g1D, g2D, enable_collection class DataCollection(): def __init__(self, name): self.data_lim = 1000 if len(sys.argv) > 1: self.data_lim = int(sys.argv[1]) print 'Taking {} data points'.format(sys.argv[1]) self.data = np.zeros(self.data_lim) self.ctr = 0 self.name = name self._enable = False self._mean = 0 self._std_dev = 0
sys.path.append('/opt/ros/kinetic/lib/python2.7/dist-packages') import rospy except Exception as e: print(e) pass import std_msgs.msg from ambf_msgs.msg import ObjectState, ObjectCmd # from kuka7DOF_Spatial_Transform_case import * from InverseDynamicsModule import * from geometry_msgs.msg import Vector3 # our global variables state_msg = ObjectState() active = True pos = [] # ROS Subscriber callback function def get_joint_values(data): global state_msg, active, pos pos = data.joint_positions pos=np.asarray(pos) # print(pos) # print "state: ", currentState return pos def main():
# \author <http://www.aimlab.wpi.edu> # \author <*****@*****.**> # \author Adnan Munawar # \version 0.1 # */ # //============================================================================== from ambf_msgs.msg import ObjectState, ObjectCmd import rospy from geometry_msgs.msg import Pose from PyKDL import Rotation, Frame, Vector global openhmd_state, occulus_state, occulus_state_valid, openhmd_state_valid openhmd_state = Pose() occulus_state = ObjectState() occulus_cmd = ObjectCmd() occulus_state_valid = False openhmd_state_valid = False def openhmd_cb(msg): global openhmd_state, openhmd_state_valid openhmd_state = msg openhmd_state_valid = True def occulus_cb(msg): global occulus_state, occulus_state_valid occulus_state = msg occulus_state_valid = True