def __init__(self, name, side, robot, command_ip): # Setup parameters for this servo namespace = "~joints/{}_joint/".format(name) self.name = "{}_{}_joint".format(side, name) command_topic = "{}/{}_{}_controller/command".format(robot, side, name) if command_ip == "local": self.local = True ip = None else: self.local = False ip = command_ip # Setup joint variables self.index = 0 self.position = 0.0 self.desired = 0.0 self.velocity = 0.0 # Setup publishers and subscribers if self.local: self.pub = rospy.Publisher(command_topic, Float64, queue_size=5) else: self.pub = rC.RosMsg("ws4py", "ws://" + ip + ":9090/", "pub", command_topic, "std_msgs/Float64", pack_float64)
def comms_initialization(self): """ Intialize communications for the deliberative module. """ # Setup publishers # NOTE: For now, we assume that the left arm movegroup is on the # same computer as the DL. The right arm movegroup is assumed # to be on a separate computer, requiring websocket comms. self.DL_to_left_HL_pub = rospy.Publisher("/deliberative/to_hl", DLtoHL, queue_size=1) self.DL_to_right_HL_pub = rC.RosMsg('ws4py', self.connection, "pub", "/deliberative/to_hl", "rse_dam_msgs/DLtoHL", rpack.pack_DL_to_HL) self.DL_to_Op_pub = rospy.Publisher("/deliberative/to_op", DLtoOp, queue_size=1) # Setup subscribers self.left_HL_to_DL_sub = rospy.Subscriber("/left_habitual/to_dl", HLtoDL, self.left_hl_cb) self.right_HL_to_DL_sub = rospy.Subscriber("/right_habitual/to_dl", HLtoDL, self.right_hl_cb) self.Op_to_DL_sub = rospy.Subscriber("/operator/to_dl", OptoDL, self.op_cb)
def __init__(self): # Get inputs self.connection = sys.argv[1] # Initialize node rospy.init_node("clock_publisher", anonymous=True) # Initialize cleanup for this node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Set the publishing rate (1 kHz) self.rate = rospy.Rate(1000.0) # '/clock' Subscriber rospy.Subscriber("/clock", Clock, self.time_cb) # Setup ROSbridge publisher ws = rC.RosMsg("ws4py", self.connection, "pub", "/clock", "rosgraph_msgs/Clock", pack_time) # Run publisher rospy.loginfo("Clock publisher initialized") self.publish_clock(ws)
def comms_initialization(self): """ Intialize communications for the MoveIt interface. """ # Setup publishers # NOTE: For now, we assume that the left arm movegroup is on the # same computer as the DL. The right arm movegroup is assumed # to be on a separate computer, requiring websocket comms. if (self.side == "left"): self.HL_to_DL_pub = rospy.Publisher("left_HLtoDL", HLtoDL, queue_size=1) elif (self.side == "right"): self.HL_to_DL_pub = rC.RosMsg('ws4py', "ws://" + self.connection + ":9090/", "pub", "right_HLtoDL", "rse_dam_msgs/HLtoDL", rpack.pack_HL_to_DL) self.HL_to_RL_pub = rospy.Publisher("HLtoRL", HLtoRL, queue_size=1) # Setup subscribers self.DL_to_HL_sub = rospy.Subscriber("{}_DLtoHL".format(self.side), DLtoHL, self.dl_to_hl_cb) self.RL_to_HL_sub = rospy.Subscriber("RLtoHL", RLtoHL, self.rl_to_hl_cb) self.joint_state_sub = rospy.Subscriber( "{}/{}_arm/joint_states".format(self.robot, self.side), JointState, self.joint_state_cb)
def __init__(self): # Get Inputs self.connection = sys.argv[1] left_arm = sys.argv[2] self.robot = sys.argv[3] self.joint_names = [ "joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "gripper_joint" ] if (left_arm == "True"): self.arm = "left" elif (left_arm == "False"): self.arm = "right" # Setup storage lists self.ws = [] self.subs = [] self.commands = [None] * 6 callbacks = [ self.joint_1_cb, self.joint_2_cb, self.joint_3_cb, self.joint_4_cb, self.joint_5_cb, self.gripper_joint_cb ] # Initialize node rospy.init_node("arm_command_publisher") # Initialize cleanup for this node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Setup publishing rate (100 Hz) self.rate = rospy.Rate(100.0) # Setup ROSbridge publishers and ROSsubscribers for i in range(0, len(self.joint_names)): topic = self.robot + "/" + self.arm + "_" + self.joint_names[ i] + "_controller/command" self.subs.append(rospy.Subscriber(topic, Float64, callbacks[i])) self.ws.append( rC.RosMsg("ws4py", self.connection, "pub", topic, "std_msgs/Float64", pack_float64)) # Run publishers rospy.sleep(5) rospy.loginfo("Joint command publisher initialized") self.publish_joint_commands()
def init_comms(self): """ Initialize the communication interfaces for the module. May use ROS patterns or ROSbridge library. TODO: Test """ # Message topic names robot_state = "{}/state_estimate".format(self.robot) left_joint_state = "{}/left_arm/joint_states".format(self.robot) tf = "{}/left_arm/tf".format(self.robot) right_joint_state = "{}/right_arm/joint_states".format(self.robot) # Setup ROS Subscribers self.left_joint_sub = rospy.Subscriber(left_joint_state, JointState, self.left_joint_state_cb) self.right_joint_sub = rospy.Subscriber(right_joint_state, JointState, self.right_joint_state_cb) # Setup local ROS Publishers self.local_state_pub = rospy.Publisher(robot_state, DualArmState, queue_size=1) rospy.loginfo("Local ROS subscribers and publishers initialized") # Setup ROSbridge Publishers self.pubs = [] for key, ip in self.connections.items(): if ip == self.local_ip: pass else: pass new_ws_pub = rC.RosMsg("ws4py", "ws://"+ip+":9090/", "pub", robot_state, "rse_dam_msgs/DualArmState", pack_dual_arm_state) self.pubs.append(new_ws_pub) # Display communication information to terminal rospy.loginfo("{} rosbridge publishers".format(len(self.pubs)))
def __init__(self): # Initialize node rospy.init_node("clock_publisher", anonymous=True) # Initialize cleanup for this node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Set the publishing rate (1 kHz) self.rate = rospy.Rate(1000.0) # Initialize variables self.clock_time = None # '/clock' Subscriber rospy.Subscriber("/clock", Clock, self.time_cb) # Setup ROSbridge publisher(s) self.pubs = [] local_ip = rospy.get_param("~connections/sim") for name in rospy.get_param("~connections", dict()).keys(): ip = rospy.get_param("~connections/" + name) if ip == local_ip: pass else: self.pubs.append( rC.RosMsg("ws4py", "ws://" + ip + ":9090/", "pub", "/clock", "rosgraph_msgs/Clock", pack_time)) # Make sure there are actual publishing needs, or else shut down if len(self.pubs) == 0: rospy.logwarn( "All clock publish destinations are local. Clock publisher not needed" ) exit() # Run publisher rospy.loginfo("Simulation clock publisher initialized") self.main()
def comms_initialization(self): """ Intialize communications for the MoveIt interface. """ # Setup publishers # NOTE: For now, we assume that the left arm movegroup is on the # same computer as the DL. The right arm movegroup is assumed # to be on a separate computer, requiring websocket comms. if (self.side == "left"): self.HL_to_DL_pub = rospy.Publisher("/left_habitual/to_dl", HLtoDL, queue_size=1) elif (self.side == "right"): self.HL_to_DL_pub = rC.RosMsg('ws4py', self.connection, "pub", "/right_habitual/to_dl", "rse_dam_msgs/HLtoDL", rpack.pack_HL_to_DL) # Setup subscribers self.DL_to_HL_sub = rospy.Subscriber("/deliberative/to_hl", DLtoHL, self.dl_to_hl_cb)
def __init__(self): # Get inputs self.connection = sys.argv[1] left_arm = sys.argv[2] self.joint_names = ["right_joint_1", "right_joint_2", "right_joint_3", "right_joint_4", "right_joint_5", "right_gripper_joint"] self.robot = sys.argv[3] if (left_arm=="True"): self.arm = "left" elif (left_arm=="False"): self.arm = "right" # Storage lists/dicts self.header = {} self.position = [] self.velocity = [] self.effort = [] # Initialize node rospy.init_node("{}_state_publisher".format(self.robot), anonymous=True) # Initialize cleanup for this node rospy.on_shutdown(self.cleanup) # Get a lock self.lock = thread.allocate_lock() # Setup publishing rate (100 Hz) self.rate = rospy.Rate(100.0) # 'robot/joint_state' Subscriber rospy.Subscriber("{}/joint_states".format(self.robot), JointState, self.get_joint_states) # Setup ROSbridge publisher ws = rC.RosMsg("ws4py", self.connection, "pub", "{}/joint_states".format(self.robot), "sensor_msgs/JointState", pack_jointstate) # Run publisher rospy.loginfo("Arm state publisher initialized") self.publish_arm_state(ws)
def __init__(self): # Inititialize node rospy.init_node("arm_state_publisher", anonymous=True) # Get a lock self.lock = thread.allocate_lock() # Get parameters robot = rospy.get_param("~robot") side = rospy.get_param("~side") self.side = side local_ip = rospy.get_param("~connections/sim") dest_ip = rospy.get_param("~connections/{}_arm".format(side)) if dest_ip == local_ip: rospy.loginfo("Publishing commands to local ROS environment") ip = "local" # TODO: rework this else: rospy.loginfo("Publishing commands to ws://{}:9090/".format(command_ip)) # Parse out joint information from param server joint_dict = rospy.get_param("~joints") arm_joints = ["{}_{}_joint".format(self.side, joint) for joint in joint_dict["arm"]] eef_joints = ["{}_{}_joint".format(self.side, joint) for joint in joint_dict["eef"]] self.joints = arm_joints + eef_joints # Setup arm state storage variable self.arm_state = JointState() self.arm_state.name = self.joints self.arm_state.position = [0.0]*len(self.joints) self.arm_state.velocity = [0.0]*len(self.joints) self.arm_state.effort = [0.0]*len(self.joints) self.j = [None]*len(self.joints) # Initialize cleanup for this node rospy.on_shutdown(self.cleanup) # Message storage variable self.robot_joint_state = None # Set rate (100 Hz) self.rate = rospy.Rate(100.0) # Get topics whole_joint_state = "{}/joint_states".format(robot) arm_joint_state = "{}/{}_joint_state".format(robot, side) # Setup ROS Subscriber rospy.Subscriber(whole_joint_state, JointState, self.joint_state_cb) # Setup ROSbridge publisher or local publisher if ip == "local": self.local = True self.pub = rospy.Publisher(arm_joint_state, JointState, queue_size=1) else: self.local = False self.pub = rC.RosMsg("ws4py", "ws://"+ip+":9090/", "pub", arm_joint_state, "sensor_msgs/JointState", pack_joint_state) # Run publisher rospy.loginfo("State publisher for {} arm initialized".format(side)) self.main()
from geometry_msgs.msg import Pose, PoseArray, PoseStamped from moveit_msgs.msg import RobotTrajectory from trajectory_msgs.msg import JointTrajectoryPoint from sensor_msgs.msg import JointState if __name__ == "__main__": pub_or_sub = raw_input("publishing(p) or subscribing(s)? ") connection = "ws://192.168.0.51:9090/" #left-to-right #connection = "ws://192.168.0.50:9090/" #right-to-left if (pub_or_sub=="s"): # sub test ws = rC.RosMsg('ws4py', connection, 'sub', '/string_test', 'std_msgs/String', compack.unpack_string) out = None while (out is None): out = ws.receive() if out is None: print("No data recieved yet") else: print("Data recieved!") print(out) time.sleep(0.5) elif (pub_or_sub=="p"): # pub test header = Header()