def startNode(self): """ Start the node and the clients. """ rospy.init_node (NODE_NAME, anonymous=False, log_level=rospy.INFO) # Create the clients self._pathfinderClient = PathfinderClient() self._asservClient = AsservClient() self._localizerClient = LocalizerClient() self._collisionsClient = CollisionsClient(self._callbackEmergencyStop, self._callbackAsservResume) self._mapClient = MapClient() # Create action servers and topic publisher self._actionSrv_Dogoto = actionlib.ActionServer(FULL_NODE_NAME + "/goto_action", DoGotoAction, self._handleDoGotoRequest, auto_start=False) self._actionSrv_doGotoWaypoint = actionlib.ActionServer(FULL_NODE_NAME + "/gotowaypoint_action", DoGotoWaypointAction, self._handleDoGotoWaypointRequest, auto_start=False) self._statusPublisher = rospy.Publisher(FULL_NODE_NAME + "/status", Status, queue_size=1) # Launch the node self._actionSrv_Dogoto.start() self._actionSrv_doGotoWaypoint.start() rospy.loginfo ("Ready to navigate!") self._currentPlan = Plan(self._asservClient, self._pathfinderClient, self._planResultCallback, self._updateStatus) self._updateStatus() # Tell ai/game_manager the node initialized successfuly. StatusServices(NODE_NAMESPACE, NODE_NAME).ready(True) rospy.spin ()
def __init__(self): self._as = actionlib.ActionServer("/count_until",CountUntilAction, self.on_goal, self.on_cancel,auto_start=False) self._as.start() rospy.loginfo("Action Server has been started") # self._cancel_request = False self._cancel_goals = {}
def __init__(self): self._action_name = "motion_planner_server" self._action_server = actionlib.ActionServer( self._action_name, QuadMoveAction, self._new_goal, cancel_cb=self._cancel_request, auto_start=False) self._goal_tasks = [] self._current_task = None self._current_goal = None self._cancel_requested = False self._lock = threading.RLock() # Start action server last to avoid race condition self._action_server.start() self._task_dict = { 'takeoff': TakeoffTask, 'land': LandTask, 'xyztranslate': XYZTranslationTask, 'track_roomba': TrackRoombaTask, 'hit_roomba': HitRoombaTask, 'block_roomba': BlockRoombaTask, 'hold_position': HoldPositionTask, 'height_recovery': HeightRecoveryTask, 'velocity_test': VelocityTask, 'test_task': TestTask }
def __init__(self, name): self._action_name = name self._as = actionlib.ActionServer(self._action_name, offload.msg.RoutePlanAction, goal_cb=self.goal_cb, auto_start=False) self._as.start()
def __init__(self, position_manager): self.pos_manager = position_manager self.server = actionlib.ActionServer( 'niryo_one/commander/robot_action', RobotMoveAction, self.on_goal, self.on_cancel, auto_start=False) self.current_goal_handle = None self.learning_mode_on = False self.joystick_enabled = False # Load validation params from rosparams self.validation = rospy.get_param( "/niryo_one/robot_command_validation") rospy.loginfo( "Waiting for service 'niryo_one/commander/execute_command'...") rospy.wait_for_service('niryo_one/commander/execute_command') rospy.loginfo("Service found : 'niryo_one/commander/execute_command'") self.is_active_server = rospy.Service('niryo_one/commander/is_active', GetInt, self.callback_is_active) self.learning_mode_subscriber = rospy.Subscriber( '/niryo_one/learning_mode', Bool, self.callback_learning_mode) self.joystick_enabled_subscriber = rospy.Subscriber( '/niryo_one/joystick_interface/is_enabled', Bool, self.callback_joystick_enabled)
def __init__(self): #TODO: Replace next line with creation of placement controller class, it should still be called controller self.controller=PlacementController() self.server=actionlib.ActionServer("placement_step", PlacementStepAction, goal_cb=self.execute_cb,cancel_cb=self.cancel, auto_start=False) self.server.start() self.previous_goal=None
def __init__(self, robot, jointStatePublisher, jointPrefix, nodeName, goal_time_tolerance=None): self.robot = robot self.jointPrefix = jointPrefix self.prefixedJointNames = [ s + self.jointPrefix for s in TrajectoryFollower.jointNames ] self.jointStatePublisher = jointStatePublisher self.timestep = int(robot.getBasicTimeStep()) self.motors = [] self.sensors = [] for name in TrajectoryFollower.jointNames: self.motors.append(robot.getDevice(name)) self.sensors.append(robot.getDevice(name + '_sensor')) self.sensors[-1].enable(self.timestep) self.goal_handle = None self.trajectory = None self.joint_goal_tolerances = [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] self.server = actionlib.ActionServer(nodeName + "follow_joint_trajectory", FollowJointTrajectoryAction, self.on_goal, self.on_cancel, auto_start=False)
def __init__(self, robot_interface): # type: (HiwinRobotInterface) -> None self.robot_interface = robot_interface self.robot_name = self.robot_interface.name self.server = actionlib.ActionServer(self.robot_name + "/follow_joint_trajectory", FollowJointTrajectoryAction, self.on_goal, self.on_cancel, auto_start=False) rospy.Subscriber("manipulator/taskstart", String, callback=self.taskstart_topic_callback, queue_size=10) self.pub_feedback_states = rospy.Publisher( '/feedback_states', FollowJointTrajectoryFeedback, queue_size=10) self.goal_handle = None self.start_time = 0 self.received_trajectory = None # Store last received trajectory self.trajectory_to_execute = None # Once executed, it will be deleted self.target = None self.update_timer = rospy.Timer(rospy.Duration(self.RATE), self._update) self.__keep_running = False self.__thread = None
def __init__(self, robot, jointStatePublisher, jointPrefix, goal_time_tolerance=None): self.robot = robot self.jointPrefix = jointPrefix self.prefixedJointNames = [ s + self.jointPrefix for s in TrajectoryFollowerGripper.jointNames ] print self.prefixedJointNames self.jointStatePublisher = jointStatePublisher self.timestep = int(robot.getBasicTimeStep()) self.motors = [] self.sensors = [] for name in TrajectoryFollowerGripper.internjointNames: self.motors.append(robot.getMotor(name)) self.sensors.append(robot.getPositionSensor(name + '_sensor')) self.sensors[-1].enable(self.timestep) print 'init motor: {}'.format(self.motors) self.goal_handle = None self.last_point_sent = True self.trajectory = None self.joint_goal_tolerances = [0.05] self._max_joint_limit = 0.8 self._stroke = 0.085 self.server = actionlib.ActionServer( "gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction, self.on_goal, self.on_cancel, auto_start=False)
def __init__(self, robot, jointStatePublisher, jointPrefix, armName, goal_time_tolerance=None): self.jointNames = [ armName + '_joint1', armName + '_joint2', armName + '_joint3', armName + '_joint4', armName + '_joint5', armName + '_joint6', armName + '_joint7' ] self.robot = robot self.jointPrefix = jointPrefix self.prefixedJointNames = [s + self.jointPrefix for s in self.jointNames] self.jointStatePublisher = jointStatePublisher self.timestep = int(robot.getBasicTimeStep()) self.motors = [] self.sensors = [] for name in self.jointNames: self.motors.append(robot.getMotor(name)) self.sensors.append(robot.getPositionSensor(name + '_sensor')) self.sensors[-1].enable(self.timestep) self.goal_handle = None self.trajectory = None self.joint_goal_tolerances = [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] self.server = actionlib.ActionServer(armName + "_arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction, self.on_goal, self.on_cancel, auto_start=False)
def __init__(self): # Initialize the Action Server self._as = actionlib.ActionServer('/action_ros_iot', msgRosIotAction, self.on_goal, self.on_cancel, auto_start=False) ''' * self.on_goal - It is the fuction pointer which points to a function which will be called when the Action Server receives a Goal. * self.on_cancel - It is the fuction pointer which points to a function which will be called when the Action Server receives a Cancel Request. ''' # Read and Store IoT Configuration data from Parameter Server param_config_iot = rospy.get_param('config_pyiot') self._config_mqtt_server_url = param_config_iot['mqtt']['server_url'] self._config_mqtt_server_port = param_config_iot['mqtt']['server_port'] self._config_mqtt_sub_topic = param_config_iot['mqtt']['topic_sub'] self._config_mqtt_pub_topic = param_config_iot['mqtt']['topic_pub'] self._config_mqtt_qos = param_config_iot['mqtt']['qos'] self._config_mqtt_sub_cb_ros_topic = param_config_iot['mqtt'][ 'sub_cb_ros_topic'] self._config_google_apps_spread_sheet_id = param_config_iot[ 'google_apps']['spread_sheet_id'] print(param_config_iot) # Initialize ROS Topic Publication # Incoming message from MQTT Subscription will be published on a ROS Topic (/ros_iot_bridge/mqtt/sub). # ROS Nodes can subscribe to this ROS Topic (/ros_iot_bridge/mqtt/sub) to get messages from MQTT Subscription. self._handle_ros_pub = rospy.Publisher( self._config_mqtt_sub_cb_ros_topic, msgMqttSub, queue_size=10) # Subscribe to MQTT Topic (eyrc/xYzqLm/iot_to_ros) which is defined in 'config_iot_ros.yaml'. # self.mqtt_sub_callback() function will be called when there is a message from MQTT Subscription. ret = iot.mqtt_subscribe_thread_start(self.mqtt_sub_callback, self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_pub_topic, self._config_mqtt_qos) if (ret == 0): rospy.loginfo("MQTT Subscribe Thread Started") else: rospy.logerr("Failed to start MQTT Subscribe Thread") ret2 = iot.mqtt_subscribe_thread_start(self.mqtt_sub_callback_start, self._config_mqtt_server_url, self._config_mqtt_server_port, self._config_mqtt_sub_topic, self._config_mqtt_qos) if (ret2 == 0): rospy.loginfo("MQTT Subscribe Thread Started") else: rospy.logerr("Failed to start MQTT Subscribe Thread") # Start the Action Server self._as.start() rospy.loginfo("Started ROS-IoT Bridge Action Server.")
def __init__(self): self._as = actionlib.ActionServer('/action_pick', msgPickPlaceAction, self.on_goal, self.on_cancel, auto_start=False) cam = Camera1() rospy.sleep(5) self.stored_in = cam.package_info self.camera_info = cam.store self.ur5_1 = pick('ur5_1') self.ur5_2 = place('ur5_2', self.stored_in) lst_joint_angles_1 = [ 0.13683998732796088, -2.4423682116751824, -1.0174361654883883, -1.2520121813310645, 1.57071913195598, 0.13659848941419117 ] self.ur5_1.hard_set_joint_angles(lst_joint_angles_1, 5) self.ur5_2.hard_set_joint_angles(lst_joint_angles_1, 5) self.count = 0 self.goal_info = {} self._goal_handles = {} self.colour = {'Medicine': 'red', 'Food': 'yellow', 'Clothes': 'green'} self.item = {'red': 'Medicine', 'yellow': 'Food', 'green': 'Clothes'} self.priority = {'red': 'HP', 'yellow': 'MP', 'green': 'LP'} self.cost = {'red': 450, 'yellow': 250, 'green': 150} self._as.start() rospy.loginfo("Started Pick Action Server.")
def __init__(self, robot, jointStatePublisher, nodeName, goal_time_tolerance=None): self.robot = robot self.jointPrefix = 'panda_joint' self.prefixedJointNames = [ self.jointPrefix + str(s + 1) for s in range(7) ] self.gripper = ['panda_finger_joint1', 'panda_finger_joint2'] # self.prefixedJointNames = self.prefixedJointNames + self.gripper self.jointStatePublisher = jointStatePublisher self.timestep = int(robot.getBasicTimeStep()) self.motors = [] self.sensors = [] self.gripperMotors = [] self.gripperSensors = [] for name in self.prefixedJointNames: self.motors.append(robot.getDevice(name)) self.sensors.append(robot.getDevice(name + '_sensor')) self.sensors[-1].enable(self.timestep) for name in self.gripper: self.gripperMotors.append(robot.getDevice(name)) self.gripperSensors.append(robot.getDevice(name + '_sensor')) self.gripperSensors[-1].enable(self.timestep) self.goal_handle = None self.trajectory = None self.gripper_goal_handle = None self.gripper_trajectory = None self.joint_goal_tolerances = [ 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.01, 0.01 ] self.jointServer = actionlib.ActionServer( nodeName + "position_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectoryAction, self.on_goal, self.on_cancel, auto_start=False) self.griperServer = actionlib.ActionServer( nodeName + "panda_hand_controller/follow_joint_trajectory", FollowJointTrajectoryAction, self.gripper_goal, self.gripper_cancel, auto_start=False)
def __init__(self, name): self._action_server_name = name self._as = actionlib.ActionServer(self._action_server_name, pnp_msgs.msg.PNPAction, self.execute_cb, auto_start=False) self._as.start() rospy.loginfo('%s: Action Server started' % self._action_server_name)
def __init__(self, disable_ft=False): self.controller = ProcessController(disable_ft) self.server = actionlib.ActionServer("process_step", ProcessStepAction, goal_cb=self.execute_cb, cancel_cb=self.cancel, auto_start=False) #TODO make goal cancelling of goal handle start here self.server.start()
def __init__(self): # Give the node a name # rospy.init_node('heimarobot_move_distance', anonymous=False) # Set rospy to execute a shutdown function when terminating the script # rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? # self.rate = rospy.get_param('~rate', 20) self.rate = rospy.Rate(20) # Set the distance to travel self.test_distance = 0.5 # meters self.speed_vel = 0.15 # meters per second self.speed_angle = 0.15 self.safe_distance = 0.20 self.tolerance = rospy.get_param('~tolerance', 0.01) # meters self.odom_linear_scale_correction = rospy.get_param( '~odom_linear_scale_correction', 1.0) self.start_test_distance = False self.start_test_angle = False # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) self.sub = rospy.Subscriber("scan_filtered", LaserScan, self.laser_detect) self.pub = rospy.Publisher("scan_front", LaserScan, queue_size=10) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') self.pub_img = rospy.Publisher('/opencv_img', Image, queue_size=2) self.sub_vision = rospy.Subscriber('/car_img', Image, self.vision_detect) self.vision_mat = None # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) self.is_moving = False self.goal_queue = Queue.Queue() threading.Thread(target=self.do_goal).start() self.server = actionlib.ActionServer("/slam/move", CarMoveAction, self.goal_cb, self.cancel_cb, False) self.server.start()
def __init__(self): rospy.init_node('~sound_player') self.device_name = rospy.get_param("~device_name") self._sound_queue = SoundQueue() self._current_gh = None self._current_process = None self.server = actionlib.ActionServer('~sound_request', SoundRequestAction, self._callback_goal, self._callback_cancel, False)
def __init__(self, name): rospy.loginfo("Grip Controller initializin' here...") self._action_name = name self._as = actionlib.ActionServer( self._action_name, control_msgs.msg.GripperCommandAction, self.on_goal, self.on_cancel, auto_start=False) # execute_cb=self.execute_cb, self._as.start()
def __init__(self, sequence_manager): self.server = actionlib.ActionServer('niryo_one/sequences/execute', SequenceAction, self.on_goal, self.on_cancel, auto_start=False) self.current_goal_handle = None self.seq_manager = sequence_manager
def __init__(self, name): rospy.loginfo("Arm Controller initializin' here...") self._action_name = name self._as = actionlib.ActionServer( self._action_name, control_msgs.msg.FollowJointTrajectoryAction, self.on_goal, self.on_cancel, auto_start=False) # execute_cb=self.execute_cb, self._as.start()
def __init__(self): self.blockly_generator = BlocklyCodeGenerator() self.server = actionlib.ActionServer('niryo_one/blockly_action', BlocklyAction, self.on_goal, self.on_cancel, auto_start=False) self.current_goal_handle = None
def __init__(self, sequence_manager, logger): self.gauss_ros_logger = logger self.server = actionlib.ActionServer('gauss/sequences/execute', SequenceAction, self.on_goal, self.on_cancel, auto_start=False) self.seq_code_executor = SequenceCodeExecutor() self.current_goal_handle = None self.seq_manager = sequence_manager
def __init__(self, sequence_manager): self.server = actionlib.ActionServer('niryo_one/sequences/execute', SequenceAction, self.on_goal, self.on_cancel, auto_start=False) self.seq_code_executor = SequenceCodeExecutor() self.current_goal_handle = None self.seq_manager = sequence_manager self.break_point_subscriber = rospy.Subscriber("/niryo_one/blockly/break_point", Int32, self.callback_break_point)
def __init__(self, server_name): self._action_server = actionlib.ActionServer(server_name, TaskRequestAction, self._new_goal, cancel_cb=self._cancel_request, auto_start=False) self._current_request = None self._new_request = None self._cancel_requested = False self._lock = threading.RLock() self._action_server.start()
def __init__(self, name): self._action_name = name self._as = actionlib.ActionServer(self._action_name, roslaunch_axserver.msg.launchAction, self.execute_cb, self.cancel_cb, auto_start=False) self._as.start() self.p = {} # a lock to mutex access to the master object self.master_lock = threading.Lock() rospy.loginfo('Server is up')
def __init__(self, position_manager, trajectory_manager): self.trajectory_manager = trajectory_manager self.pos_manager = position_manager moveit_commander.roscpp_initialize(sys.argv) # Load all the sub-commanders self.move_group_arm = MoveGroupArm() self.arm_commander = ArmCommander(self.move_group_arm) self.tool_commander = ToolCommander() self.stop_trajectory_server = rospy.Service( 'niryo_one/commander/stop_command', SetBool, self.callback_stop_command) self.reset_controller_pub = rospy.Publisher( '/niryo_one/steppers_reset_controller', Empty, queue_size=1) # robot action server self.server = actionlib.ActionServer( 'niryo_one/commander/robot_action', RobotMoveAction, self.on_goal, self.on_cancel, auto_start=False) self.current_goal_handle = None self.learning_mode_on = False self.joystick_enabled = False self.hardware_status = None self.is_active_server = rospy.Service('niryo_one/commander/is_active', GetInt, self.callback_is_active) self.learning_mode_subscriber = rospy.Subscriber( '/niryo_one/learning_mode', Bool, self.callback_learning_mode) self.joystick_enabled_subscriber = rospy.Subscriber( '/niryo_one/joystick_interface/is_enabled', Bool, self.callback_joystick_enabled) self.hardware_status_subscriber = rospy.Subscriber( '/niryo_one/hardware_status', HardwareStatus, self.callback_hardware_status) self.validation = rospy.get_param( "/niryo_one/robot_command_validation") self.parameters_validation = ParametersValidation(self.validation) # arm velocity self.max_velocity_scaling_factor = 100 self.max_velocity_scaling_factor_pub = rospy.Publisher( '/niryo_one/max_velocity_scaling_factor', Int32, queue_size=10) self.timer = rospy.Timer(rospy.Duration(1.0), self.publish_arm_max_velocity_scaling_factor) self.max_velocity_scaling_factor_server = rospy.Service( '/niryo_one/commander/set_max_velocity_scaling_factor', SetInt, self.callback_set_max_velocity_scaling_factor)
def __init__(self): self.server = actionlib.ActionServer('nao_run_script', RunScriptAction, self.execute, self.cancel, False) self.goalHandle = {} self.goal = {} self.pm = {} self.cancelled = {} self.executing = {} self.filename = {} self.result = {} self.user_id = {} self.bagfile = {} self.start_time = {} self.server.start()
def __init__(self): self.feedback = ProjectedGraspingFeedback() self.result = ProjectedGraspingResult() self.goal_received = False self.object_to_grasp = False self.action_name = 'projected_grasping_server' self.action_server = actionlib.ActionServer(self.action_name, ProjectedGraspingAction, self.action_callback, self.cancel_cb, auto_start=False) self.action_server.start() self.action_status = GoalStatus()
def __init__(self, ac_name, ac_type, cb, immediate_success=True): """ If immediate_success is False, the user will have to call reply() to set_succeeded() the goal. Otherwise, the goal will be automatically set to succeeded after the callback return. """ self._cb = cb self._opt_cancel_cb = None self._ghdl = None self._immed = immediate_success self._ac = actionlib.ActionServer(ac_name, ac_type, auto_start=False, goal_cb=self._goal_cb, cancel_cb=self._cancel_cb) self._ac.start()
def __init__(self): self._as = actionlib.ActionServer('/count_until', CountUntilAction, self.on_goal, self.on_cancel, auto_start=False) self._as.start() rospy.loginfo("Action Server has been started.") self._cancel_goals = {} self._goal_queue = [] w = threading.Thread(name="queue_worker", target=self.run_queue) w.start() rospy.loginfo("Run queue thread has been started")