def __init__(self, joint_name, controller_name, joint_names):
rospy.loginfo("Creating interactive marker for: " + str(joint_name))
self.joint_name = joint_name
self.controller_name = controller_name
self.joint_names = joint_names
self.base_name = joint_name.replace('_joint', '')
# Get joint limits for the marker
free_joints = get_joint_limits()
if not free_joints.has_key(joint_name):
rospy.logerr("Could not find '" + joint_name + "' in the free joints dictionary, there will be no Interactive Marker.")
return
if free_joints[joint_name]["has_position_limits"]:
self.upper_joint_limit = float(free_joints[joint_name]["max_position"])
self.lower_joint_limit = float(free_joints[joint_name]["min_position"])
rospy.loginfo("Setting joint limits for " + self.base_name +
":\nUpper: " + str(self.upper_joint_limit) +
"\nLower: " + str(self.lower_joint_limit) )
else:
rospy.logwarn("No joint limits found for " + joint_name + " setting up dummys.")
self.upper_joint_limit = 4.0
self.lower_joint_limit = -4.0
self.ims = InteractiveMarkerServer("ims_" + self.base_name)
self.makeRotateMarker(self.base_name)
self.ims.applyChanges()
# Publisher for the joint
cmd_topic = "/" + controller_name + "/command"
self.pub = rospy.Publisher(cmd_topic, JointTrajectory)
rospy.loginfo("Done, now you should spin!")
def __init__(self, joint_name, controller_name, joint_names):
rospy.loginfo("Creating interactive marker for: " + str(joint_name))
self.joint_name = joint_name
self.controller_name = controller_name
self.joint_names = joint_names
self.base_name = joint_name.replace('_joint', '')
# Get joint limits for the marker
free_joints = get_joint_limits()
if not free_joints.has_key(joint_name):
rospy.logerr(
"Could not find '" + joint_name +
"' in the free joints dictionary, there will be no Interactive Marker."
)
return
if free_joints[joint_name]["has_position_limits"]:
self.upper_joint_limit = float(
free_joints[joint_name]["max_position"])
self.lower_joint_limit = float(
free_joints[joint_name]["min_position"])
rospy.loginfo("Setting joint limits for " + self.base_name +
":\nUpper: " + str(self.upper_joint_limit) +
"\nLower: " + str(self.lower_joint_limit))
else:
rospy.logwarn("No joint limits found for " + joint_name +
" setting up dummys.")
self.upper_joint_limit = 4.0
self.lower_joint_limit = -4.0
self.ims = InteractiveMarkerServer("ims_" + self.base_name)
self.makeRotateMarker(self.base_name)
self.ims.applyChanges()
# Publisher for the joint
cmd_topic = "/" + controller_name + "/command"
self.pub = rospy.Publisher(cmd_topic, JointTrajectory)
rospy.loginfo("Done, now you should spin!")
def __init__(self):
rospy.loginfo("Initializing InteractiveJointTrajCtrl")
ctl_mngr_srv = rospy.ServiceProxy(CONTROLLER_MNGR_SRV, ListControllers)
rospy.loginfo("Connecting to " + CONTROLLER_MNGR_SRV)
ctl_mngr_srv.wait_for_service()
rospy.loginfo("Connected.")
self.rviz_config_str = ""
req = ListControllersRequest()
resp = ctl_mngr_srv.call(req)
#ListControllersResponse()
ctl_mngr_srv.close()
joints_dict = get_joint_limits()
publishers_dict = {}
# dictionary that contains
# key: head_controller
# value:
# joints: ['head_1_joint', 'head_2_joint']
# publisher: pub
# ims: [interactive_marker_server_head_1_joint, interactive_marker_server_head_2_joint]
for cs in resp.controllers: # For every controller, create a random spammer
# cs is a string with the name of the controller, now check if has command interface
#published_topics = rostopic.rospy.get_published_topics() #for some reason is not the same than
# doing the bash call
ps = subprocess.Popen(["rostopic", "list" ], stdout=subprocess.PIPE)
controller_related_topics = subprocess.check_output(('grep', cs), stdin=ps.stdout) #, stdout=subprocess.PIPE)
#print "controller_related_topics to " + cs + ": " + str(controller_related_topics)
for topic_name in controller_related_topics.split():
#print "topic_name: " + topic_name
if "command" in topic_name:
# We need to check if there is a query state service to ask him
# what joints does this controller control
ps2 = subprocess.Popen(["rosservice", "list"], stdout=subprocess.PIPE)
with_query_controllers = subprocess.check_output(('grep', 'query_state'), stdin=ps2.stdout)
#print "services with query_state: " + str(with_query_controllers)
joint_names = []
for srv_name in with_query_controllers.split():
#print "service: " + srv_name
if cs in srv_name:
print "Controller '" + cs + "' has topic /command interface and service /query_state interface!"
joint_names = get_joints_of_controller(cs)
if not joint_names:
continue
else:
publishers_dict[cs] = {}
publishers_dict[cs]['joints'] = joint_names
cmd_topic = "/" + cs + "/command"
publishers_dict[cs]['pub'] = rospy.Publisher(cmd_topic, JointTrajectory)
publishers_dict[cs]['ims'] = []
for joint_name in publishers_dict[cs]['joints']:
publishers_dict[cs]['ims'].append( LinkInteractiveMarker(joint_name, cs, joint_names))
curr_im_rviz_cfg = self.create_im_config_rviz_block(joint_name)
self.rviz_config_str += curr_im_rviz_cfg
self.save_rviz_config(self.rviz_config_str)
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc:
ns = self._cm_ns.rsplit('/', 1)[0]
if ns not in self._cm_checked:
try:
for jnt, lims in get_joint_limits(
description=rospy.get_param(
'{}/robot_description'.format(
ns))).iteritems():
self._robot_joint_limits[jnt] = lims
self._cm_checked.add(ns)
except KeyError:
rospy.logwarn(
'Could not find a valid robot_description parameter in namespace {}'
.format(ns))
try:
for jnt, lims in get_joint_limits(
description=rospy.get_param(
'robot_description')).iteritems():
self._robot_joint_limits[jnt] = lims
except KeyError:
rospy.logwarn(
'Could not find robot_description parameter')
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info)
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def __init__(self):
rospy.loginfo("Initializing InteractiveJointTrajCtrl")
ctl_mngr_srv = rospy.ServiceProxy(CONTROLLER_MNGR_SRV, ListControllers)
rospy.loginfo("Connecting to " + CONTROLLER_MNGR_SRV)
ctl_mngr_srv.wait_for_service()
rospy.loginfo("Connected.")
self.rviz_config_str = ""
req = ListControllersRequest()
resp = ctl_mngr_srv.call(req)
#ListControllersResponse()
ctl_mngr_srv.close()
joints_dict = get_joint_limits()
publishers_dict = {}
# dictionary that contains
# key: head_controller
# value:
# joints: ['head_1_joint', 'head_2_joint']
# publisher: pub
# ims: [interactive_marker_server_head_1_joint, interactive_marker_server_head_2_joint]
for cs in resp.controller: # For every controller, create a publisher
#cs = ControllerState()
print "cs.name: " + str(cs.name),
if len(cs.resources
) > 0: # If the controller controls any joint only
print "...controls joints!"
publishers_dict[cs.name] = {}
publishers_dict[cs.name]['joints'] = cs.resources
cmd_topic = "/" + cs.name + "/command"
publishers_dict[cs.name]['pub'] = rospy.Publisher(
cmd_topic, JointTrajectory)
publishers_dict[cs.name]['ims'] = []
for joint_name in publishers_dict[cs.name]['joints']:
publishers_dict[cs.name]['ims'].append(
LinkInteractiveMarker(joint_name, cs.name,
cs.resources))
curr_im_rviz_cfg = self.create_im_config_rviz_block(
joint_name)
self.rviz_config_str += curr_im_rviz_cfg
self.save_rviz_config(self.rviz_config_str)
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits() # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info);
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def _update_jtc_list(self):
# Clear controller list if no controller information is available
if not self._list_controllers:
self._widget.jtc_combo.clear()
return
# List of running controllers with a valid joint limits specification
# for _all_ their joints
running_jtc = self._running_jtc_info()
if running_jtc and not self._robot_joint_limits:
self._robot_joint_limits = get_joint_limits() # Lazy evaluation
valid_jtc = []
for jtc_info in running_jtc:
has_limits = all(name in self._robot_joint_limits
for name in _jtc_joint_names(jtc_info))
if has_limits:
valid_jtc.append(jtc_info);
valid_jtc_names = [data.name for data in valid_jtc]
# Update widget
update_combo(self._widget.jtc_combo, sorted(valid_jtc_names))
def __init__(self, joint_name):
rospy.loginfo("Creating interactive marker for: " + str(joint_name))
self.base_name = joint_name.replace('_joint', '')
# Get joint limits for the marker
free_joints = get_joint_limits()
if free_joints[joint_name]["has_position_limits"]:
self.upper_joint_limit = float(free_joints[joint_name]["max_position"])
self.lower_joint_limit = float(free_joints[joint_name]["min_position"])
rospy.loginfo("Setting joint limits for " + self.base_name +
":\nUpper: " + str(self.upper_joint_limit) +
"\nLower: " + str(self.lower_joint_limit) )
else:
rospy.logwarn("No joint limits found for " + joint_name + " setting up dummys.")
self.upper_joint_limit = 4.0
self.lower_joint_limit = -4.0
self.ims = InteractiveMarkerServer("ims_" + self.base_name)
self.makeRotateMarker(self.base_name)
self.ims.applyChanges()
# Publisher for the joint
#self.pub = rospy.Publisher()
rospy.loginfo("Done, now you should spin!")
def __init__(self):
rospy.loginfo("Initializing InteractiveJointTrajCtrl")
ctl_mngr_srv = rospy.ServiceProxy(CONTROLLER_MNGR_SRV, ListControllers)
rospy.loginfo("Connecting to " + CONTROLLER_MNGR_SRV)
ctl_mngr_srv.wait_for_service()
rospy.loginfo("Connected.")
self.rviz_config_str = ""
req = ListControllersRequest()
resp = ctl_mngr_srv.call(req)
#ListControllersResponse()
ctl_mngr_srv.close()
joints_dict = get_joint_limits()
publishers_dict = {}
# dictionary that contains
# key: head_controller
# value:
# joints: ['head_1_joint', 'head_2_joint']
# publisher: pub
# ims: [interactive_marker_server_head_1_joint, interactive_marker_server_head_2_joint]
for cs in resp.controller: # For every controller, create a publisher
#cs = ControllerState()
print "cs.name: " + str(cs.name),
if len(cs.resources) > 0: # If the controller controls any joint only
print "...controls joints!"
publishers_dict[cs.name] = {}
publishers_dict[cs.name]['joints'] = cs.resources
cmd_topic = "/" + cs.name + "/command"
publishers_dict[cs.name]['pub'] = rospy.Publisher(cmd_topic, JointTrajectory)
publishers_dict[cs.name]['ims'] = []
for joint_name in publishers_dict[cs.name]['joints']:
publishers_dict[cs.name]['ims'].append( LinkInteractiveMarker(joint_name, cs.name, cs.resources))
curr_im_rviz_cfg = self.create_im_config_rviz_block(joint_name)
self.rviz_config_str += curr_im_rviz_cfg
self.save_rviz_config(self.rviz_config_str)
def __init__(self, joint_name):
rospy.loginfo("Creating interactive marker for: " + str(joint_name))
self.base_name = joint_name.replace('_joint', '')
# Get joint limits for the marker
free_joints = get_joint_limits()
if free_joints[joint_name]["has_position_limits"]:
self.upper_joint_limit = float(
free_joints[joint_name]["max_position"])
self.lower_joint_limit = float(
free_joints[joint_name]["min_position"])
rospy.loginfo("Setting joint limits for " + self.base_name +
":\nUpper: " + str(self.upper_joint_limit) +
"\nLower: " + str(self.lower_joint_limit))
else:
rospy.logwarn("No joint limits found for " + joint_name +
" setting up dummys.")
self.upper_joint_limit = 4.0
self.lower_joint_limit = -4.0
self.ims = InteractiveMarkerServer("ims_" + self.base_name)
self.makeRotateMarker(self.base_name)
self.ims.applyChanges()
# Publisher for the joint
#self.pub = rospy.Publisher()
rospy.loginfo("Done, now you should spin!")
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource', 'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = get_joint_limits() # For all robot joints
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
joint_names = resp.name
return joint_names
if __name__ == '__main__':
rospy.init_node('cntl_crazier_')
ctl_mngr_srv = rospy.ServiceProxy(CONTROLLER_MNGR_SRV, ListControllers)
rospy.loginfo("Connecting to " + CONTROLLER_MNGR_SRV)
ctl_mngr_srv.wait_for_service()
rospy.loginfo("Connected.")
req = ListControllersRequest()
resp = ctl_mngr_srv.call(req)
#ListControllersResponse()
ctl_mngr_srv.close()
joints_dict = get_joint_limits()
#print "this is joints_dict: " + str(joints_dict)
threads = []
for cs in resp.controllers: # For every controller, create a random spammer
# cs is a string with the name of the controller, now check if has command interface
#published_topics = rostopic.rospy.get_published_topics() #for some reason is not the same than
# doing the bash call
ps = subprocess.Popen(["rostopic", "list" ], stdout=subprocess.PIPE)
controller_related_topics = subprocess.check_output(('grep', cs), stdin=ps.stdout) #, stdout=subprocess.PIPE)
#print "controller_related_topics to " + cs + ": " + str(controller_related_topics)
for topic_name in controller_related_topics.split():
#print "topic_name: " + topic_name
if "command" in topic_name:
# We need to check if there is a query state service to ask him
# what joints does this controller control
ps2 = subprocess.Popen(["rosservice", "list"], stdout=subprocess.PIPE)
def __init__(self, context):
super(JointTrajectoryController, self).__init__(context)
self.setObjectName('JointTrajectoryController')
# Create QWidget and extend it with all the attributes and children
# from the UI file
self._widget = QWidget()
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_joint_trajectory_controller'),
'resource',
'joint_trajectory_controller.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('JointTrajectoryControllerUi')
# Setup speed scaler
speed_scaling = DoubleEditor(1.0, 100.0)
speed_scaling.spin_box.setSuffix('%')
speed_scaling.spin_box.setValue(50.0)
speed_scaling.spin_box.setDecimals(0)
speed_scaling.setEnabled(False)
self._widget.speed_scaling_layout.addWidget(speed_scaling)
self._speed_scaling_widget = speed_scaling
speed_scaling.valueChanged.connect(self._on_speed_scaling_change)
self._on_speed_scaling_change(speed_scaling.value())
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Initialize members
self._jtc_name = [] # Name of selected joint trajectory controller
self._cm_ns = [] # Namespace of the selected controller manager
self._joint_pos = {} # name->pos map for joints of selected controller
self._joint_names = [] # Ordered list of selected controller joints
self._robot_joint_limits = get_joint_limits() # For all robot joints
# Timer for sending commands to active controller
self._update_cmd_timer = QTimer(self)
self._update_cmd_timer.setInterval(1000.0 / self._cmd_pub_freq)
self._update_cmd_timer.timeout.connect(self._update_cmd_cb)
# Timer for updating the joint widgets from the controller state
self._update_act_pos_timer = QTimer(self)
self._update_act_pos_timer.setInterval(1000.0 /
self._widget_update_freq)
self._update_act_pos_timer.timeout.connect(self._update_joint_widgets)
# Timer for controller manager updates
self._list_cm = ControllerManagerLister()
self._update_cm_list_timer = QTimer(self)
self._update_cm_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_cm_list_timer.timeout.connect(self._update_cm_list)
self._update_cm_list_timer.start()
# Timer for running controller updates
self._update_jtc_list_timer = QTimer(self)
self._update_jtc_list_timer.setInterval(1000.0 /
self._ctrlrs_update_freq)
self._update_jtc_list_timer.timeout.connect(self._update_jtc_list)
self._update_jtc_list_timer.start()
# Signal connections
w = self._widget
w.enable_button.toggled.connect(self._on_jtc_enabled)
w.jtc_combo.currentIndexChanged[str].connect(self._on_jtc_change)
w.cm_combo.currentIndexChanged[str].connect(self._on_cm_change)
self._cmd_pub = None # Controller command publisher
self._state_sub = None # Controller state subscriber
self._list_controllers = None
