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()