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