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