def main():
rosclient = roslibpy.Ros('10.244.1.176', port=9090)
rosclient.on_ready(
lambda: print('is ROS connected: ', rosclient.is_connected))
rosclient.run()
mc = rmc.MobileClient(rosclient,
lambda *s: print(f'【reached】 result: {s}'))
mc.wait_for_ready()
print(f'pos: {mc.position}, map_shape: {mc.map_data.shape}')
ga_map = MapExploration('map_exploration', mc)
ga_are = AreaSelection('area_selection', mc)
ga_obs = Observation('observation', mc)
pool_grounded = GroundedActionPool([ga_map, ga_are, ga_obs])
ga_sched = GAScheduler('top_scheduler', mc, pool_grounded)
mc.start()
# ------------------------------------------------
for i in range(10):
s = ga_sched.step()
pos = s.candidates['pos'][s.selected_cand_id]
ori = s.candidates['ori'][s.selected_cand_id]
mc.set_goal(pos, ori)
mc.wait_for_goal_accepted()
# ------------------------------------------------
mc.stop()
rosclient.terminate()
print(f'finished {s.candidates}')
def run_service(rosbridge_url: str, service_name: str, service_type: str):
client = roslibpy.Ros(rosbridge_url)
client.run()
service = roslibpy.Service(client, service_name, service_type)
request = roslibpy.ServiceRequest()
_ = service.call(request)
client.terminate()
def __init__(self, arm_names, upstream_topic, host, port, trigger_topic, drop_rate):
self.upstream_topic = upstream_topic
self.trigger_topic = trigger_topic
self.drop_rate = drop_rate
self.cnt = 0
# Creating rosbridge connection
self.ros = roslibpy.Ros(host=host, port=port, is_secure=False)
self.ros.on_ready(self.connect_cb)
self.arms = []
# Constructing RobotArmDVRK instance for each arm
for name in arm_names:
if 'SUJ_PSM' in name:
self.arms.append(RobotArmSUJPSM(name, self.ros))
elif 'SUJ_ECM' in name:
self.arms.append(RobotArmSUJECM(name, self.ros))
elif 'ECM' in name:
self.arms.append(RobotArmECM(name, self.ros))
elif 'PSM' in name:
self.arms.append(RobotArmPSM(name, self.ros))
# Advertising rosbridge topic
self.topic = roslibpy.Topic(self.ros, self.upstream_topic, 'sensor_msgs/JointState')
self.topic.advertise()
# Start loop
self.ros.run_forever()
def __init__(self, session_name=''):
self.client = roslibpy.Ros(host=settings.ROS_HOST,
port=settings.ROS_PORT)
self.client.run()
self.session_name = session_name
self.srv_status = roslibpy.Service(self.client, '/rosout/get_loggers',
'roscpp/GetLoggers')
self.srv_path = roslibpy.Service(self.client,
'/deepbuilder/robot/plan_path',
'/deepbuilder/ro_plan_path')
self.srv_update_state_mesh = roslibpy.Service(
self.client, '/deepbuilder/robot/update_state_mesh',
'/deepbuilder/ro_update_state_mesh')
self.srv_update_compressed_mesh = roslibpy.Service(
self.client, '/deepbuilder/robot/update_compressed_mesh',
'/deepbuilder/ro_update_compressed_mesh')
self.srv_get_tags = roslibpy.Service(self.client,
'/deepbuilder/sensing/get_tags',
'/deepbuilder/se_get_tags')
self.srv_collect_state = roslibpy.Service(
self.client, '/deepbuilder/sensing/collect_state',
'/deepbuilder/se_collect_state')
#self.srv_get_joint_states = roslibpy.Service(self.client, '/deepbuilder/robot/get_robot_state', '/deepbuilder/ro_get_robot_state')
self.srv_move_path = roslibpy.Service(self.client,
'/deepbuilder/robot/move_path',
'/deepbuilder/ro_move_path')
self.srv_print_path = roslibpy.Service(
self.client, '/deepbuilder/robot/print_path',
'/deepbuilder/ro_print_path')
def run_me():
ros_client = roslibpy.Ros(host='161.253.72.254', port=9090)
print("Connecting...")
param_sender = roslibpy.Param(ros_client, '/pub_joint_param')
def set_param():
while True:
if not ros_client.is_connected:
print('Wait a minute, ROS connection is down.')
break
updating_angles = read_data(input_file_name)
param_sender.set(updating_angles)
time.sleep(0.01)
def read_data(file_name):
# use with to close file after reading automatically
# read only
with open(file_name, "r") as text_file:
# read strings that are seperated with space
# a dummy tail is needed in this txt file
# because by default, txt file will have a \n in the end
string_line0 = text_file.read().split(' ')
# convert strings into float numbers
float_line0 = [
float(string_line0[0]),
float(string_line0[1]),
float(string_line0[2]),
float(string_line0[3]),
float(string_line0[4]),
float(string_line0[5])
]
return float_line0
ros_client.on_ready(set_param, run_in_thread=True)
ros_client.run_forever()
def __init__(self):
# base robot ip and port
self.__base_port = rospy.get_param('~base_port', 9090)
self.__base_ip = rospy.get_param('~base_address', 'scout-mini-02.local')
# robot name, each robot should have different name
self.__robot_name = rospy.get_param('~robot_name', 'scout-mini-02')
self.__robot_frame_id = rospy.get_param('~robot_frame_id', 'base_footprint')
self.__load_swarm_trans()
self.__current_command = 'dismiss'
self.__tf_listener = tf.TransformListener()
self.__ros_client = roslibpy.Ros(self.__base_ip, self.__base_port)
self.__joy_command = roslibpy.Topic(self.__ros_client, 'yocs_cmd_vel_mux/input/joy_cmd', 'geometry_msgs/Twist')
self.__swarm_command = roslibpy.Topic(self.__ros_client, 'swarm_command', 'swarm_msgs/SwarmCommand')
self.__swarm_heartbeat = roslibpy.Topic(self.__ros_client, 'swarm_hearbeat', 'swarm_msgs/SwarmHeartbeat')
# self.__move_base_pub = rospy.Publisher('swarm_goal', PoseStamped, queue_size=30)
self.__move_base_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=30)
self.__cmd_pub = rospy.Publisher('yocs_cmd_vel_mux/input/navigation_cmd', Twist, queue_size=30)
# self.__cmd_pub = rospy.Publisher('test_cmd', Twist, queue_size=30)
self.__target_pose = PoseStamped()
self.__swarm_base_pose = Pose()
self.__ros_client.on_ready(self.__start_sending, run_in_thread=True)
self.__ros_client.on_ready(self.__start_receiving, run_in_thread=True)
self.__ros_client.run()
rospy.loginfo('Swarm client started')
async def setup_rosbridge(self) : # connect to rosbridge self.rosbridge_client = roslibpy.Ros(host=self.rosbridge_address, port=self.rosbridge_port) await self.attempt(self.rosbridge_client.run, check=(lambda x : self.rosbridge_client.is_connected), sync=True, timeout=1) # create client for frame service self.frame_service = roslibpy.Service(self.rosbridge_client, self.frame_req_service_name, self.frame_req_msg_type) # subscribe to frame topic self.frame_subscriber = roslibpy.Topic(self.rosbridge_client, self.frame_resp_topic_name, self.frame_resp_msg_type) self.frame_subscriber.subscribe(lambda msg: self.subscriber_callback(msg)) # setup publisher for storage goal topic self.storage_publisher = roslibpy.Topic(self.rosbridge_client, self.storage_req_topic_name, self.storage_req_msg_type) self.storage_publisher.advertise() # subscribe to storage result topic self.storage_subscriber = roslibpy.Topic(self.rosbridge_client, self.storage_resp_topic_name, self.storage_resp_msg_type) self.storage_subscriber.subscribe(lambda msg: self.subscriber_callback(msg)) # setup publisher for registration goal topic self.registration_publisher = roslibpy.Topic(self.rosbridge_client, self.registration_req_topic_name, self.registration_req_msg_type) self.registration_publisher.advertise() # subscribe to registration result topic self.registration_subscriber = roslibpy.Topic(self.rosbridge_client, self.registration_resp_topic_name, self.registration_resp_msg_type) self.registration_subscriber.subscribe(lambda msg: self.subscriber_callback(msg))
def main():
# rc = roslibpy.Ros('localhost', port=9090)
client = roslibpy.Ros('10.244.1.176', port=9090)
client.on_ready(lambda: print('is ROS connected: ', client.is_connected))
client.run()
# topic_o = '/odometry/filtered'
topic_o = '/odom'
ms = MobileClient(client, lambda r: print('reached goal', r), odom_topic=topic_o)
ms.wait_for_ready(timeout=80)
print('map_header: ', ms.map_header)
print('odom:', ms.position, [math.degrees(v) for v in quaternion.as_euler_angles(ms.orientation)])
## you can set goal any time not only after call start().
ms.start() ## make goal appended to queue, executable
# ms.set_goal_relative_xy(0.5, 0, True) ## set scheduler a goal that go ahead 0.5 from robot body
# ms.set_goal_relative_xy(-0.5, 1) ## relative (x:front:-0.5, y:left:1)
ms.set_goal(ms.get_vec_q(-0.4,-0.6,0), ms.get_rot_q(0,0,math.pi/2))
time.sleep(30)
# ms.set_goal_relative_xy(0.5, 0, True)
# time.sleep(30)
ms.stop()
print('finish')
client.terminate()
def setup_ros(self):
self.ros = roslibpy.Ros(host=self.rosparams['host'],
port=self.rosparams['port'])
self.ros.run()
self.tree_description_listener = roslibpy.Topic(
self.ros, '/abtm/yaml_tree_description', 'std_msgs/String')
self.tree_description_listener.subscribe(self.on_tree)
self.exception_listener = roslibpy.Topic(self.ros, '/abtm/exception',
'std_msgs/String')
self.exception_listener.subscribe(self.on_exception)
self.states_listener = roslibpy.Topic(self.ros,
'/abtm/yaml_state_changes',
'std_msgs/String')
self.states_listener.subscribe(self.on_states)
self.vars_listener = roslibpy.Topic(self.ros, '/abtm/yaml_var_changes',
'std_msgs/String')
self.vars_listener.subscribe(self.on_vars)
self.command_publisher = roslibpy.Topic(self.ros, '/abtm/yaml_command',
'std_msgs/String')
self.main_file_publisher = roslibpy.Topic(self.ros, '/abtm/main_file',
'std_msgs/String')
def init_ros_client():
global ROS_CLIENT, ROS_TOPICS, ROS_SERVICES
if ROS_CLIENT is None:
ROS_CLIENT = roslibpy.Ros(host='mani.local', port=9090)
ROS_CLIENT.run()
pull_topics_n_services()
return ROS_CLIENT
def __init__(self, master_name :str, port :int):
self.master_name = master_name
self.port = port
self.client = rlp.Ros(self.master_name, port=self.port) #rlp.Ros('10.244.1.117', port=9090)\
self.client.on_ready(lambda: print('is ROS connected: ', self.client.is_connected))
self.client.run()
self.services = {} # {service_name: rlp.Service}
def ros_run(self, conn):
self.conn = conn
uav_number = conn.recv()
ros = roslibpy.Ros(host='localhost', port=9090)
ros.on_ready(lambda: print('Is ROS Connected?', ros.is_connected))
roslibpy.Topic(ros, '/cv{0}/output_raw'.format(uav_number),
'sensor_msgs/Image').subscribe(self.callback)
ros.run_forever()
def __init__(self, host="localhost", port=9090, ready_callback=None,
log_data=False):
self.ready_callback = ready_callback
self.log_data = log_data
self.ros = ros = roslibpy.Ros(host=host, port=port)
ros.on_ready(self.start_publisher_thread)
ros.connect()
def __init__(self):
self.client = roslibpy.Ros(host='localhost', port=9090)
self.client.run()
self.service = roslibpy.Service(self.client, 'get_images', 'exp_demo/ObjectArray')
self.publisher = roslibpy.Topic(self.client, '/indicate', 'geometry_msgs/PointStamped')
self.publisher.advertise()
def __init__(self,robot_id):
# init HMM model
working_dir = os.path.dirname(os.path.realpath(__file__))
# Load per-situation HMMs
self.pblHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pblHMM_k_3_hf.npz")
self.pbrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pbrHMM_k_3_hf.npz")
self.rotlHMM = hmms.DtHMM.from_file(working_dir+"/../Models/rotlHMM_k_3_hf.npz")
self.rotrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/rotrHMM_k_3_hf.npz")
self.pclHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pclHMM_k_3_hf.npz")
self.pcrHMM = hmms.DtHMM.from_file(working_dir+"/../Models/pcrHMM_k_3_hf.npz")
# Configuration parameters
self.rejection_KL_thresh = 0.025
N_nodes = 81
self.classes = ["PBL", "PBR", "ROTL", "ROTR", "PCL", "PCR"]
# Functions for mapping QTC_C states to integers
# Create list of QTC_C states so that indices can be used as integer state IDs compatible with HMM library
QTC_symbols = []
for i in range(0, 4):
QTC_symbols.append("-")
QTC_symbols.append("0")
QTC_symbols.append("+")
# print("QTC symbols:", QTC_symbols[:3])
self.QTC_C_states = list(combinations(QTC_symbols, 4))
self.QTC_C_states = [state[0] + state[1] + state[2] + state[3] for state in self.QTC_C_states]
self.QTC_C_states = list(np.unique(self.QTC_C_states))
# init param
self.prev_time = time.time()
self.qtc_seq = []
# init ros
ros_client = roslibpy.Ros(host='localhost', port=9090)
ros_client.run()
print("ROS connected:", ros_client.is_connected)
# read parameters
print("Robot Id :", robot_id)
rejection_thresh_param = roslibpy.Param(ros_client, "/robot"+str(robot_id)+"/qsr/situation_rejection_threshold")
self.rejection_KL_thresh = rejection_thresh_param.get()
print("rejection_KL_thresh:",self.rejection_KL_thresh )
# define topics
self.qtcTopic = roslibpy.Topic(ros_client, "/robot"+str(robot_id)+"/qsr/qtc_state", "std_msgs/String")
self.sitTopic = roslibpy.Topic(ros_client, "/robot"+str(robot_id)+"/qsr/situation_predictions", "std_msgs/String")
# define subscribers
self.qtcTopic.subscribe(lambda message: self.qtc_update_callback(message))
while ros_client.is_connected:
time.sleep(1)
ros_client.terminate()
def main_(bag_file):
fo = open("output_image.txt", "w")
fo.write("")
fo.close()
fo = open("output.txt", "w")
fo.write("")
fo.close()
bag_file = '/data/' + str(bag_file)
played = False
def bagPlayed(Bagdata):
global played
if (Bagdata):
played = False
fo = open("output.txt", "a")
time.sleep(1)
fo.write("bag finished playing")
print("bag finished playing")
fo.close()
client = roslibpy.Ros(host=os.environ['HOST'], port=8080)
client.run()
client.on_ready(lambda: print('Is ROS connected?', client.is_connected))
imu_listener = roslibpy.Topic(client, '/mavros/imu/data',
'sensor_msgs/Imu')
imu_listener.subscribe(lambda IMUdata: ImuCallback([
float(IMUdata['linear_acceleration']['x']),
float(IMUdata['linear_acceleration']['y']),
float(IMUdata['linear_acceleration']['z'])
], IMUdata['header']))
#image_listener = roslibpy.Topic(client, '/pylon_camera_node/image_raw', 'sensor_msgs/Image')
image_listener = roslibpy.Topic(client, '/resizedImage',
'sensor_msgs/TimeReference')
image_listener.subscribe(lambda Imagedata: ImageCallback(Imagedata))
bag_listener = roslibpy.Topic(client, '/bagEnd', 'std_msgs/Bool')
bag_listener.subscribe(lambda Bagdata: bagPlayed(Bagdata))
bag_player = roslibpy.Topic(client, '/bagStart', 'std_msgs/String')
#To play Bag file - change above bag path and call funtion below (now only runs once)
##funtion start
if (client.is_connected and (not played)):
bag_player.publish(roslibpy.Message({'data': bag_file}))
played = True
##funtion end
try:
while True:
pass
except KeyboardInterrupt:
client.terminate()
def __init__(self, *args, **kwargs):
self.ros = roslibpy.Ros(host=HOST, port=PORT)
self.ros_odom = roslibpy.Topic(self.ros, '/odom', 'nav_msgs/Odometry')
self.ros_vel = roslibpy.Topic(self.ros, '/cmd_vel_mux/input/teleop', 'geometry_msgs/Twist')
self.step = 0
self.prev_robot_action = np.array([0,0])
super().__init__(*args, **kwargs)
if REAL:
self.start_ros()
def __init__(self):
self.roslibpy_client = roslibpy.Ros(host='192.168.2.76', port=9099)
self.roslibpy_client.run()
self.pos_sender = roslibpy.Topic(self.roslibpy_client,
'/iiwa_joint_position',
'iiwa_msgs/JointPosition')
self.vel_sender = roslibpy.Topic(self.roslibpy_client,
'/iiwa_joint_velocity',
'iiwa_msgs/JointVelocity')
self.pose_receiver = roslibpy.Topic(self.roslibpy_client, 'FX_msg',
'geometry_msgs/PoseStamped')
self.pose_receiver.subscribe(self.callback_basepose)
rospy.init_node('communication_node')
self.vel_sub = rospy.Subscriber('iiwa/state/JointVelocity',
JointVelocity, self.call_back_vel)
self.pos_sub = rospy.Subscriber('iiwa/state/JointPosition',
JointPosition, self.call_back_pos)
self.base_pose_pub = rospy.Publisher('FX_msg_roslibpy',
PoseStamped,
queue_size=10)
self.count = 0
self.base_pose = PoseStamped()
self.joint_vel = dict()
self.joint_vel['header'] = dict()
self.joint_vel['header']['seq'] = 0
self.joint_vel['header']['frame_id'] = ''
self.joint_vel['header']['stamp'] = dict()
self.joint_vel['header']['stamp']['secs'] = 0
self.joint_vel['header']['stamp']['nsecs'] = 0
self.joint_vel['velocity'] = dict()
self.joint_vel['velocity']['a1'] = 0
self.joint_vel['velocity']['a2'] = 0
self.joint_vel['velocity']['a3'] = 0
self.joint_vel['velocity']['a4'] = 0
self.joint_vel['velocity']['a5'] = 0
self.joint_vel['velocity']['a6'] = 0
self.joint_vel['velocity']['a7'] = 0
self.joint_pos = dict()
self.joint_pos['header'] = dict()
self.joint_pos['header']['seq'] = 0
self.joint_pos['header']['frame_id'] = ''
self.joint_pos['header']['stamp'] = dict()
self.joint_pos['header']['stamp']['secs'] = 0
self.joint_pos['header']['stamp']['nsecs'] = 0
self.joint_pos['position'] = dict()
self.joint_pos['position']['a1'] = 0
self.joint_pos['position']['a2'] = 0
self.joint_pos['position']['a3'] = 0
self.joint_pos['position']['a4'] = 0
self.joint_pos['position']['a5'] = 0
self.joint_pos['position']['a6'] = 0
self.joint_pos['position']['a7'] = 0
def __init__(self):
self.cli = roslibpy.Ros(host=os.getenv('CARTOGRAPHER_HOST', CARTOGRAPHER_ROS['HOST']),
port=os.getenv('CARTOGRAPHER_PORT', CARTOGRAPHER_ROS['PORT']))
self.map_listener = None
self.submap_list_listener = None
self.submap_query_service = None
self.trajectory_query_service = None
self.time_service = None
self.trajectory_result = PoseCaller()
self.cli.run()
def __init__(self, model_pth, srv_name, srv_type, host, port=9090, img_height=480, img_width=640):
self.ros = roslibpy.Ros(host=host, port=port)
self.ros.connect()
self.model = YolactInterface(model_pth) # TODO: Change load model function
self.srv_name = srv_name
self.srv_type = srv_type
self.h = img_height
self.w = img_width
init_input = np.zeros([self.h, self.w, 3], np.uint8)
self.model.run_once(init_input) # TODO: Change inference once function
def connection_to_rosbridge(self, host, port):
if not self.__ros:
self.__ros = roslibpy.Ros(host=host, port=port)
try:
self.__ros.run()
time.sleep(1)
except:
self.__parent.print_console(
"Problème de connexion a RosBridge")
self.deconnection_to_rosbridge()
def __init__(self, host, port):
self.ros = roslibpy.Ros(host=host, port=port, is_secure=False)
self.ros.on_ready(self.connect_cb)
topic_name = "/twin/dvrk/PSM1/state_joint_current"
self.topic = roslibpy.Topic(self.ros, topic_name,
'sensor_msgs/JointState')
self.topic.advertise()
self.ros.run_forever()
def randomNumberGenerator():
client = roslibpy.Ros(host='localhost', port=9090)
client.run()
listener = roslibpy.Topic(client, '/turtle1/pose', 'turtlesim/Pose')
listener.subscribe(lambda message: socketio.emit('newnumber', {'number': message['x']}, namespace='/test'))
try:
while not thread_stop_event.isSet():
pass
finally:
client.terminate()
def __init__(self):
self.client = roslibpy.Ros(host=ROBOT_IP, port=ROBOT_PORT)
self.messenger_publisher = roslibpy.Topic(self.client,
messenger.MESSENGER_TOPIC,
messenger.MESSENGER_TYPE)
self.controller_publisher = roslibpy.Topic(self.client,
controller.CONTROLLER_TOPIC,
controller.CONTROLLER_TYPE)
self.voice_publisher = roslibpy.Topic(self.client,
voice_assistant.VOICE_TOPIC,
voice_assistant.VOICE_TYPE)
def attempt_connection(self):
self.server_connection = roslibpy.Ros(host=self.ip, port=self.port)
rospy.loginfo('Attempting to connect to: ' + self.ip)
self.server_connection.run(timeout=10)
if self.namespace == '/':
if self.server_connection.is_connected:
result = self.register_drone(self.server_connection, self.name)
self.namespace = "/drone_" + str(result['id'])
rospy.loginfo('Connected! Launching MAVRos!')
self.launch_mavros()
self.topic_publisher = TopicPublisher(self.namespace,
self.server_connection)
def __init__(self, ip: str):
client = roslibpy.Ros(host=ip, port=9090)
client.run()
listener = roslibpy.Topic(client, '/img', 'std_msgs/String')
listener.subscribe(self.rec_handler)
try:
while True:
pass
except KeyboardInterrupt:
client.terminate()
def setup_client(self):
print(
f'Setting up rover {self.rover_id} on {self.bridge_host}:{self.bridge_port}'
)
try:
self.client = roslibpy.Ros(host=self.bridge_host,
port=self.bridge_port)
self.client.run()
return True
except:
print(f'Failed to connect to {self.rover_id}')
self.client = None
return False
def start_client(self, server_addr='localhost', server_port=9090):
"""Start the Roslib client and connect to server.
Also initializes the ActionClientWorkarounds."""
self.client = roslibpy.Ros(host=server_addr, port=server_port)
self.client.run()
print(self.client.is_connected)
self.mission_uploader = roslibpy.actionlib.ActionClient(
self.client, "isaacs_server/upload_mission",
'isaacs_server/UploadMissionAction')
self.drone_controller = roslibpy.actionlib.ActionClient(
self.client, "isaacs_server/control_drone",
'/isaacs_server/ControlDroneAction')
def __init__(self,host = '192.168.12.1',port = 9090):
''' Constructor for this class. '''
self.port = port
self.host = host
# Standard Command Template
self.command = {}
self.command_template = {
"move_command": 0,
"move_type": 0,
"ovr": 0,
"delay": 0,
"remote_tool": 0,
"cartesian_linear_speed": 0.0,
"target": {
"data_type": 0,
"cartesian_data": {"x": 0.0, "y": 0.0, "z": 0.0, "a": 0.0, "e": 0.0, "r": 0.0, "config_flags": ''},
"joints_mask": 0,
"joints_data": [0]},
"via": {
"data_type": 0,
"cartesian_data": {"x": 0.0, "y": 0.0, "z": 0.0, "a": 0.0, "e": 0.0, "r": 0.0, "config_flags": ''},
"joints_mask": 0,
"joints_data": [0]},
"tool": {"x": 0.0, "y": 0.0, "z": 0.0, "a": 0.0, "e": 0.0, "r": 0.0},
"frame": {"x": 0.0, "y": 0.0, "z": 0.0, "a": 0.0, "e": 0.0, "r": 0.0}}
# Methods Properties
self.Verbose = True
# Joints
self.jointStateValues = dict()
# KeyButton for JOG-Messages
self.Mode = 0
#self.JointArray = array('f',[0,0,0,0,0,0,0])
self.JointArray = [0,0,0,0,0,0,0]
self.JointJSON = json.dumps(self.JointArray)
self.KeyButton = 0
self.Mode = MoveType.JOINT.value
self.Joint_Selected = 1
client = roslibpy.Ros(self.host, self.port) # Wi-Fi
client.run()
if client.is_connected == False:
print('WebSocket is busy, disconnect the e.DO App')
else:
print('Connected with e.DO')
self.JointInit = roslibpy.Topic(client, '/bridge_init', 'edo_core_msgs/JointInit')
self.JointReset = roslibpy.Topic(client, '/bridge_jnt_reset', 'edo_core_msgs/JointReset')
self.JointCalibration = roslibpy.Topic(client, '/bridge_jnt_calib', 'edo_core_msgs/JointCalibration')
self.MovementCommand = roslibpy.Topic(client, '/bridge_move', 'edo_core_msgs/MovementCommand')
self.JogCommand = roslibpy.Topic(client, '/bridge_jog', 'edo_core_msgs/MovementCommand',queue_size = 1)
self.MovementFeedback = roslibpy.Topic(client, '/machine_movement_ack', 'edo_core_msgs/MovementFeedback',throttle_rate=200)
self.CartesianPosition = roslibpy.Topic(client, '/cartesian_pose', 'edo_core_msgs/CartesianPose',throttle_rate=200)
self.JointState = roslibpy.Topic(client, '/usb_jnt_state', 'edo_core_msgs/JointStateArray',throttle_rate=200)
def __init__(self):
self.is_connected = False
self.running_thread = True
self.lamp_mode = 0
self.dots = dotstar.DotStar(board.SCK, board.MOSI, LAMP_COUNT, brightness=MAX_BRIGHTNESS, baudrate=BAUDRATE)
self.dots.fill((0, 0, 0))
self.lamp_thread = Thread(target=self.thread_callback)
self.lamp_thread.start()
self.client = roslibpy.Ros(host="192.168.1.207", port=9090)
self.client.on_ready(lambda: self.on_connected())
self.client.run_forever()