def __init__(self): # creates the action server self._as = actionlib.SimpleActionServer("drone_sqr", TestAction, self.goal_callback, False) self._as.start()
def __init__(self): self.robot = moveit_commander.RobotCommander() self.scene = moveit_commander.PlanningSceneInterface() self.contraints = Constraints() is_initialized = False while (not is_initialized): try: self.group = moveit_commander.MoveGroupCommander("arm") is_initialized = True except RuntimeError: is_initialized = False rospy.sleep(0.5) rospy.loginfo("Initialized...") self.traj_publisher = rospy.Publisher( '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1) rospy.loginfo("============ Reference planning frame: %s" % self.group.get_planning_frame()) rospy.loginfo("============ Reference end_effector link: %s" % self.group.get_end_effector_link()) # self.sub_callback_pointcloud = rospy.Subscriber('/move_group/filtered_cloud', PointCloud2, self.callback_pointcloud) self.sub_add_obstacle = rospy.Subscriber('/add_obstacle', String, self.add_obstacle) self.sub_del_obstacle = rospy.Subscriber('/del_obstacle', String, self.del_obstacle) self.sub_del_all_obstacles = rospy.Subscriber('/del_all_obstacles', String, self.del_all_obstacles) self.action_plan_execute_pose = actionlib.SimpleActionServer( '/plan_and_execute_pose', PlanExecutePoseAction, execute_cb=self.plan_execute_pose_cb, auto_start=False) self.action_plan_execute_pose.start() self.action_plan_execute_named_pose = actionlib.SimpleActionServer( '/plan_and_execute_named_pose', PlanExecuteNamedPoseAction, execute_cb=self.plan_execute_named_pose_cb, auto_start=False) self.action_plan_execute_named_pose.start() self.action_plan_execute_pose_w_constraints = actionlib.SimpleActionServer( '/plan_and_execute_pose_w_joint_constraints', PlanExecutePoseConstraintsAction, execute_cb=self.plan_execute_pose_constraints_cb, auto_start=False) self.action_plan_execute_pose_w_constraints.start() rospy.loginfo('%s ready...' % rospy.get_name()) self.box_names_arr = [] self.box_pose = geometry_msgs.msg.PoseStamped() self.collision_objects = []
def __init__(self, name): self._as = actionlib.SimpleActionServer(name, TestAction, execute_cb=self.execute_cb, auto_start=False) self._as.start()
def __init__(self, action_name, gripper): self.gripper = gripper self.action_server = actionlib.SimpleActionServer( action_name, GripperCommandAction, self.gripper_action_execute, False) self.action_server.start()
def __init__(self): self.server = actionlib.SimpleActionServer('Drone', DroneAction, self.execute, False) self.server.start()
def action_callback(goal): global i result = waypointResult() feedback = waypointFeedback() while i < len(goal.points): goal_x = goal.points[i].x goal_y = goal.points[i].y reached_checkpt = moving_controller(goal_x, goal_y) pub.publish(vel_msg) r.sleep() if reached_checkpt: i += 1 feedback.num_completed = i feedback.total_points = len(goal.points) server.publish_feedback(feedback) if i < len(goal.points): reached_checkpt = False result.succ_msg = "Reached" server.set_succeeded(result) rospy.init_node("action_server") r = rospy.Rate(10) sub = rospy.Subscriber("/base_pose_ground_truth", Odometry, sub_callback) pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) server = actionlib.SimpleActionServer('waypoints', waypointAction, action_callback, False) server.start() rospy.spin()
def __init__(self, name): self._action_name = 'player1client' # I could possibly give the name through a yaml file # tic_tac_toe.msg.playAction is the actionSpec self._as = actionlib.SimpleActionServer(self._action_name, tic_tac_toe.msg.playAction, execute_cb=self.execute_cb, auto_start = False) # as stands for action server self._as.start()
def __init__(self): self.server = actionlib.SimpleActionServer( '/nao_world_msgs/turn_action_server', TurnAction, self.execute, False) self.server.start() rospy.loginfo("Turn Action Server started.")
def __init__(self): self.server = actionlib.SimpleActionServer("plan_to_goal", PlanToGoalAction, self.execute, False) self.server.start()
pub.publish(command) else: command.angular.z = 0 pub.publish(command) result = moveToGoalResult() result.result = True server.set_succeeded(result, "Goal reached successfully") flag = 0 print("publishing") print("target pos={} current pos:{}", (dest_x, dest_y), (x, y)) ''' Initializing the Subscriber(Odom), Publisher(cmd_vel), ActionServer(Move_to_goal) ''' if __name__ == "__main__": rospy.init_node('move_robot') command = Twist() sub = rospy.Subscriber('/odom', Odometry, get_position) print("after subscriber") pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=10) server = actionlib.SimpleActionServer('commander', moveToGoalAction, execute, False) server.start() r = rospy.Rate(1) # while not rospy.is_shutdown(): # r.sleep() rospy.spin()
def exec_music(goal): r = MusicResult() fb = MusicFeedback() for i, f in enumerate(goal.freqs): fb.remaining_steps = len(goal.freqs) - i music.publish_feedback(fb) if music.is_preempt_requested(): write_freq(0) r.finished = False music.set_preempted(r) return write_freq(f) rospy.sleep(1.0 if i >= len(goal.durations) else goal.durations[i]) r.finished = True music.set_succeeded(r) if __name__ == '__main__': rospy.init_node('buzzer') rospy.Subscriber("buzzer", UInt16, recv_buzzer) music = actionlib.SimpleActionServer('music', MusicAction, exec_music, False) music.start() rospy.on_shutdown(write_freq) rospy.spin()
print("reached target angle", target_rad, yaw) command.angular.z = 0 pub.publish(command) result = RotateToGoalResult() result.result = True server.set_succeeded(result, "Goal reached successfully") flag = 0 print("publishing") print("target_angle={} current_angle:{}", target_rad, yaw) ''' Initializing the Subscriber (Odom), Publisher (cmd_vel), ActionServer (Rotate_to_goal) ''' if __name__ == "__main__": #global pub,sub,server, command rospy.init_node('rotate_robot') command = Twist() sub = rospy.Subscriber('zed/zed_node/odom', Odometry, get_rotation) print("after subscriber") pub = rospy.Publisher('/cmd_vel_mux/input/teleop', Twist, queue_size=10) server = actionlib.SimpleActionServer('rotator', RotateToGoalAction, execute, False) server.start() r = rospy.Rate(15) # while not rospy.is_shutdown(): # r.sleep() rospy.spin()
def __init__(self): # creates the action server self._as = actionlib.SimpleActionServer("action_custom_msg_as", CustomActionMsgAction, self.goal_callback, False) self._as.start()
def __init__(self): self._tfBuffer = tf2_ros.Buffer() self._tl = tf2_ros.TransformListener(self._tfBuffer) rospy.sleep(0.5) self._starting_time = None self._goal_time = None #Global Variables Init self._success = True self._goal_action = '' self._goal_pose = [] self._feedback = robot_geoFeedback() self._result = robot_geoResult() self._goal_sel_x = 1.0 self._goal_sel_y = 1.0 self._goal_sel_z = 1.0 self._goal_force_x = 0.0 self._goal_force_y = 0.0 self._goal_force_z = 0.0 self._goal_torque_x = 0.0 self._goal_torque_y = 0.0 self._goal_torque_z = 0.0 self._current_js = [] #PICK/PLACE EXCLUSIVE self._goal_gripper_content = False self._goal_obj_reach = True #SCREW EXCLUSIVE self._goal_depth = 25.4 self._goal_lead = 1.5 self._goal_screw_direction = '' self._num_rotations = 0 self._count_rotation = 0 self._quat = np.zeros((3, 3)) #MOVE_TO_CONTACT EXCLUSIVE self._count = 0 self._rf = 0.0 #Radial Force self._current_force = geometry_msgs.msg.Vector3 #Publishers self._selection_pub = rospy.Publisher('/panda/selection', geometry_msgs.msg.Vector3, queue_size=1) self._selection_msg = geometry_msgs.msg.Vector3() self._hybrid_pose_pub = rospy.Publisher('/panda/hybrid_pose', HybridPose, queue_size=1) self._hybrid_pose_msg = HybridPose() self._subaction_pub = rospy.Publisher("/panda/commands", String, queue_size=1) self._pose_msg = geometry_msgs.msg.PoseStamped() #Subscribers self._wrench_sub = rospy.Subscriber("/panda/control_wrench", geometry_msgs.msg.Wrench, self.current_force) self._current_joint_states_sub = rospy.Subscriber( "/panda/joint_states", JointState, self.current_joint_states, queue_size=1) #Action_Server self.a_server = actionlib.SimpleActionServer( "action_as", robot_geoAction, execute_cb=self.execute_cb, auto_start=False) self.a_server.start()
def __init__(self): rospy.init_node(self.NODE_NAME) self.server = actionlib.SimpleActionServer(self.ACTION_NAME, AcousticsDataAction, self.execute, False) self.server.start() rospy.spin()
rospy.loginfo('===================Succeeded==============') tba_sas_grip.set_succeeded(result) rospy.loginfo(result) rospy.init_node('tbarm_actions') rospy.loginfo("Node started") arm_pub = rospy.Publisher("arm/move_jp", sensor_msgs.msg.JointState, queue_size=10) feedback = control_msgs.msg.FollowJointTrajectoryFeedback() tba_sas_arm = actionlib.SimpleActionServer( "turtlearm/arm_controller/follow_joint_trajectory", control_msgs.msg.FollowJointTrajectoryAction, execute_cb=execute_cb_arm, auto_start=False) tba_sas_grip = actionlib.SimpleActionServer( "turtlearm/grip_controller/follow_joint_trajectory", control_msgs.msg.FollowJointTrajectoryAction, execute_cb=execute_cb_grip, auto_start=False) tba_sas_arm.start() rospy.loginfo("AS arm started") tba_sas_grip.start() rospy.loginfo("AS grip started") result = control_msgs.msg.FollowJointTrajectoryResult()
# topics we subscribe to aux_cmd = rospy.Subscriber('aux_cmd', Int32, auxCmdCB) start_time = time.time() # extract the goal parameters speed = goal.speed secs_per_side = goal.secs_per_side.to_sec() # time.sleep(goal.secs_per_side.to_sec()) # perform the necessary actions rospy.logdebug("Drive in a square at %d speed, %d seconds per side", speed, secs_per_side) # send the result result = SquareResult() result.time_driven = rospy.Duration.from_sec(time.time() - start_time) result.updates_sent = 0 server.set_succeeded(result) rospy.init_node('ROS_SQUARE_DRIVE', log_level=rospy.DEBUG) server = actionlib.SimpleActionServer('square_drive', SquareAction, drive_square, False) server.start() rospy.spin()
def __init__(self, name): """Publish yaw and depth setpoints based on waypoints""" self._action_name = name #self.heading_offset = rospy.get_param('~heading_offsets', 5.) self.wp_tolerance = rospy.get_param('~wp_tolerance', 5.) self.depth_tolerance = rospy.get_param('~depth_tolerance', 0.5) self.base_frame = rospy.get_param('~base_frame', "sam/base_link") rpm1_cmd_topic = rospy.get_param('~rpm1_cmd_topic', '/sam/core/thruster1_cmd') rpm2_cmd_topic = rospy.get_param('~rpm2_cmd_topic', '/sam/core/thruster2_cmd') heading_setpoint_topic = rospy.get_param('~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint') depth_setpoint_topic = rospy.get_param('~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint') self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000)) #related to turbo turn self.turbo_turn_flag = rospy.get_param('~turbo_turn_flag', False) thrust_vector_cmd_topic = rospy.get_param('~thrust_vector_cmd_topic', '/sam/core/thrust_vector_cmd') yaw_feedback_topic = rospy.get_param('~yaw_feedback_topic', '/sam/ctrl/yaw_feedback') self.turbo_angle_min_deg = rospy.get_param('~turbo_angle_min', 90) self.turbo_angle_min = np.radians(self.turbo_angle_min_deg) self.turbo_angle_max = 3.0 self.flip_rate = rospy.get_param('~flip_rate', 0.5) self.rudder_angle = rospy.get_param('~rudder_angle', 0.08) self.turbo_turn_rpm = rospy.get_param('~turbo_turn_rpm', 1000) vbs_setpoint_topic = rospy.get_param('~vbs_setpoint_topic', '/sam/ctrl/vbs/setpoint') #related to velocity regulation instead of rpm #self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False) #self.vel_setpoint = rospy.get_param('~vel_setpoint', 0.5) #velocity setpoint in m/s self.roll_setpoint = rospy.get_param('~roll_setpoint', 0) vel_setpoint_topic = rospy.get_param('~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint') roll_setpoint_topic = rospy.get_param('~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint') vel_feedback_topic = rospy.get_param('~vel_feedback_topic', '/sam/dr/u_feedback') #controller services toggle_yaw_ctrl_service = rospy.get_param('~toggle_yaw_ctrl_service', '/sam/ctrl/toggle_yaw_ctrl') toggle_depth_ctrl_service = rospy.get_param('~toggle_depth_ctrl_service', '/sam/ctrl/toggle_depth_ctrl') toggle_vbs_ctrl_service = rospy.get_param('~toggle_vbs_ctrl_service', '/sam/ctrl/toggle_vbs_ctrl') toggle_speed_ctrl_service = rospy.get_param('~toggle_speed_ctrl_service', '/sam/ctrl/toggle_speed_ctrl') toggle_roll_ctrl_service = rospy.get_param('~toggle_roll_ctrl_service', '/sam/ctrl/toggle_roll_ctrl') self.toggle_yaw_ctrl = ToggleController(toggle_yaw_ctrl_service, False) self.toggle_depth_ctrl = ToggleController(toggle_depth_ctrl_service, False) self.toggle_vbs_ctrl = ToggleController(toggle_vbs_ctrl_service, False) self.toggle_speed_ctrl = ToggleController(toggle_speed_ctrl_service, False) self.toggle_roll_ctrl = ToggleController(toggle_roll_ctrl_service, False) self.nav_goal = None self.x_prev = 0 self.y_prev = 0 self.listener = tf.TransformListener() rospy.Timer(rospy.Duration(0.5), self.timer_callback) self.yaw_feedback = 0.0 rospy.Subscriber(yaw_feedback_topic, Float64, self.yaw_feedback_cb) self.vel_feedback = 0.0 rospy.Subscriber(vel_feedback_topic, Float64, self.vel_feedback_cb) self.rpm1_pub = rospy.Publisher(rpm1_cmd_topic, ThrusterRPM, queue_size=10) self.rpm2_pub = rospy.Publisher(rpm2_cmd_topic, ThrusterRPM, queue_size=10) self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10) self.depth_pub = rospy.Publisher(depth_setpoint_topic, Float64, queue_size=10) self.vel_pub = rospy.Publisher(vel_setpoint_topic, Float64, queue_size=10) self.roll_pub = rospy.Publisher(roll_setpoint_topic, Float64, queue_size=10) #TODO make proper if it works. self.vbs_pub = rospy.Publisher(vbs_setpoint_topic, Float64, queue_size=10) self.vec_pub = rospy.Publisher(thrust_vector_cmd_topic, ThrusterAngles, queue_size=10) self._as = actionlib.SimpleActionServer(self._action_name, GotoWaypointAction, execute_cb=self.execute_cb, auto_start = False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) rospy.spin()
# TODO what happens with malformed target goal??? FAILURE or INVALID_POSE # txt must be: "Aborting on goal because it was sent with an invalid quaternion" # move_base_flex get_path and move_base action clients mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction) mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path", mbf_msgs.GetPathAction) mbf_mb_ac.wait_for_server(rospy.Duration(20)) mbf_gp_ac.wait_for_server(rospy.Duration(10)) # move_base_flex dynamic reconfigure client mbf_drc = Client("move_base_flex", timeout=10) # move_base simple topic and action server mb_sg = rospy.Subscriber('/move_base_simple/goal', PoseStamped, simple_goal_cb) mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False) mb_as.start() # move_base make_plan service mb_mps = rospy.Service('~make_plan', nav_srvs.GetPlan, make_plan_cb) # move_base dynamic reconfigure server mb_drs = Server(MoveBaseConfig, mb_reconf_cb) rospy.spin()
def __init__(self): self.server = actionlib.SimpleActionServer("TEST", actionDevAAction, execute_cb=self.serverHandler, auto_start=False) self.server.start()
def __init__(self): self.server = actionlib.SimpleActionServer('add_number', AddNumberAction, self.execute, False) self.server.start()
def __init__(self, name): # Init actionserver self.vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10) self._as = actionlib.SimpleActionServer(self._action_name, navi_service.msg.BaseSlideAction, execute_cb=self.execute_cb, auto_start = False) self._as.start()
def __init__(self): NaoqiNode.__init__(self, 'pose_controller') self.connectNaoQi() # store the number of joints in each motion chain and collection, used for sanity checks self.collectionSize = {} for collectionName in [ 'Head', 'LArm', 'LLeg', 'RLeg', 'RArm', 'Body', 'BodyJoints', 'BodyActuators' ]: try: self.collectionSize[collectionName] = len( self.motionProxy.getJointNames(collectionName)) except RuntimeError: # the following is useful for old NAOs with no legs/arms rospy.logwarn('Collection %s not found on your robot.' % collectionName) # Get poll rate for actionlib (ie. how often to check whether currently running task has been preempted) # Defaults to 200ms self.poll_rate = int(rospy.get_param("~poll_rate", 0.2) * 1000) # initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running) # set to 1.0 if you want to control the robot immediately initStiffness = rospy.get_param('~init_stiffness', 0.0) # TODO: parameterize if initStiffness > 0.0 and initStiffness <= 1.0: self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5) # start services / actions: self.enableStiffnessSrv = rospy.Service("body_stiffness/enable", Empty, self.handleStiffnessSrv) self.disableStiffnessSrv = rospy.Service("body_stiffness/disable", Empty, self.handleStiffnessOffSrv) #Start simple action servers self.jointTrajectoryServer = actionlib.SimpleActionServer( "joint_trajectory", JointTrajectoryAction, execute_cb=self.executeJointTrajectoryAction, auto_start=False) self.jointStiffnessServer = actionlib.SimpleActionServer( "joint_stiffness_trajectory", JointTrajectoryAction, execute_cb=self.executeJointStiffnessAction, auto_start=False) self.jointAnglesServer = actionlib.SimpleActionServer( "joint_angles_action", JointAnglesWithSpeedAction, execute_cb=self.executeJointAnglesWithSpeedAction, auto_start=False) #Start action servers self.jointTrajectoryServer.start() self.jointStiffnessServer.start() self.jointAnglesServer.start() # only start when ALRobotPosture proxy is available if not (self.robotPostureProxy is None): self.bodyPoseWithSpeedServer = actionlib.SimpleActionServer( "body_pose_naoqi", BodyPoseWithSpeedAction, execute_cb=self.executeBodyPoseWithSpeed, auto_start=False) self.bodyPoseWithSpeedServer.start() else: rospy.logwarn( "Proxy to ALRobotPosture not available, requests to body_pose_naoqi will be ignored." ) # subscribers last: rospy.Subscriber("joint_angles", JointAnglesWithSpeed, self.handleJointAngles, queue_size=10) rospy.Subscriber("joint_stiffness", JointState, self.handleJointStiffness, queue_size=10) rospy.loginfo("nao_controller initialized")
def __init__(self, phone_face, delay=0.0, phrase_file=None, use_tts=False, voice=None): self._use_tts = use_tts in ['true', 'True', 'TRUE', '1'] if phrase_file and not len(phrase_file) > 0 and not use_tts: rospy.logerr( "CoRDial Player Error: Must specify phrase file or allow tts! Continuing without sound..." ) self._sound = False elif not phrase_file and not use_tts: rospy.logerr( "CoRDial Player Error: Must specify phrase file or allow tts! Continuing without sound..." ) self._sound = False # elif (not phrase_file or len(phrase_file)>0) and use_tts and (not ivona_secret_key or not ivona_access_key): # rospy.logerr("CoRDial Player Error: Must specify ivona keys to allow tts! Continuing without sound...") # self._sound = False # self._use_tts=False else: self._sound = True self._speech_delay_time = delay self._phone_face = phone_face self._phrases = None if phrase_file and len(phrase_file) > 0: self._sound_client = SoundClient() rospy.sleep(0.5) self._sound_client.stopAll() rospy.loginfo( "CoRDial Player reading Phrase File... this could take a while" ) with open(phrase_file, 'r') as f: s = f.read() self._phrases = yaml.load(s) rospy.loginfo("Phrase file read.") if self._use_tts: self._tts = CoRDialTTS(voice) base_topic = "" self._behavior_client = actionlib.SimpleActionClient( base_topic + 'Behavior', BehaviorAction) if self._phone_face: self._face_pub = rospy.Publisher(base_topic + 'face', FaceRequest, queue_size=10) rospy.sleep(0.5) rospy.loginfo("CoRdial Player waiting for behavior server..") self._behavior_client.wait_for_server() rospy.loginfo("CoRDial Player connected to behavior server") self._feedback.behavior = "none" rospy.loginfo("Starting CoRDial Player server...") self._server = actionlib.SimpleActionServer(base_topic + 'Player', PlayerAction, execute_cb=self.execute_cb, auto_start=False) info = "" if self._phone_face: info += ", using CoRDial face" else: info += ", NOT using CoRDial face" if self._use_tts: info += " and using TTS with voice " + voice elif self._phrases: info += " and NOT using TTS" if self._phrases: info += "; phrase file is " + phrase_file if not self._use_tts and not self._phrases: info += " and with NO sound" info += ". Delay time is " + str(self._speech_delay_time) + "s." self._server.start() rospy.loginfo("CoRDial Player server started" + info)
def __init__(self, name): self.rpm = ThrusterRPMs() """Publish yaw and depth setpoints based on waypoints""" self._action_name = name self.follower_frame = rospy.get_param('~follower_frame', '/sam_2/base_link') self.follower_odom = rospy.get_param('~follower_odom', '/sam_2/odom') # self.min_dist = rospy.get_param('~min_dist', 5.) rpm_cmd_topic = rospy.get_param('~rpm_cmd_topic', '/sam/core/rpm_cmd') heading_setpoint_topic = rospy.get_param( '~heading_setpoint_topic', '/sam/ctrl/dynamic_heading/setpoint') yaw_pid_enable_topic = rospy.get_param( '~yaw_pid_enable_topic', '/sam/ctrl/dynamic_heading/pid_enable') depth_setpoint_topic = rospy.get_param( '~depth_setpoint_topic', '/sam/ctrl/dynamic_depth/setpoint') depth_pid_enable_topic = rospy.get_param( '~depth_pid_enable_topic', '/sam/ctrl/dynamic_depth/pid_enable') self.forward_rpm = int(rospy.get_param('~forward_rpm', 1000)) #related to velocity regulation instead of rpm self.vel_ctrl_flag = rospy.get_param('~vel_ctrl_flag', False) self.vel_setpoint = rospy.get_param('~vel_setpoint', 0.5) #velocity setpoint in m/s self.roll_setpoint = rospy.get_param('~roll_setpoint', 0) vel_setpoint_topic = rospy.get_param( '~vel_setpoint_topic', '/sam/ctrl/dynamic_velocity/u_setpoint') roll_setpoint_topic = rospy.get_param( '~roll_setpoint_topic', '/sam/ctrl/dynamic_velocity/roll_setpoint') vel_pid_enable_topic = rospy.get_param( '~vel_pid_enable_topic', '/sam/ctrl/dynamic_velocity/pid_enable') self.listener = tf.TransformListener() self.rpm_pub = rospy.Publisher(rpm_cmd_topic, ThrusterRPMs, queue_size=10) self.yaw_pub = rospy.Publisher(heading_setpoint_topic, Float64, queue_size=10) self.depth_pub = rospy.Publisher(depth_setpoint_topic, Float64, queue_size=10) self.vel_pub = rospy.Publisher(vel_setpoint_topic, Float64, queue_size=10) self.roll_pub = rospy.Publisher(roll_setpoint_topic, Float64, queue_size=10) self.yaw_pid_enable = rospy.Publisher(yaw_pid_enable_topic, Bool, queue_size=10) self.depth_pid_enable = rospy.Publisher(depth_pid_enable_topic, Bool, queue_size=10) self.vel_pid_enable = rospy.Publisher(vel_pid_enable_topic, Bool, queue_size=10) self._as = actionlib.SimpleActionServer(self._action_name, MoveBaseAction, execute_cb=self.execute_cb, auto_start=False) self._as.start() rospy.loginfo("Announced action server with name: %s", self._action_name) rospy.spin()
def __init__(self): self._action_name = "/TurnTo" self._as = actionlib.SimpleActionServer(self._action_name, RobilTask.msg.RobilTaskAction, execute_cb=self.task) self._as.start()
def __init__(self, limb, reconfig_server, rate=100.0, mode='position_w_id'): self._dyn = reconfig_server self._ns = 'robot/limb/' + limb self._fjt_ns = self._ns + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._fjt_ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._limb = baxter_interface.Limb(limb) self._enable = baxter_interface.RobotEnable() self._name = limb self._cuff = baxter_interface.DigitalIO('%s_lower_cuff' % (limb,)) self._cuff.state_changed.connect(self._cuff_cb) # Verify joint control mode self._mode = mode if (self._mode != 'position' and self._mode != 'position_w_id' and self._mode != 'velocity'): rospy.logerr("%s: Action Server Creation Failed - " "Provided Invalid Joint Control Mode '%s' (Options: " "'position_w_id', 'position', 'velocity')" % (self._action_name, self._mode,)) return self._server.start() self._alive = True self._cuff_state = False # Action Feedback/Result self._fdbk = FollowJointTrajectoryFeedback() self._result = FollowJointTrajectoryResult() # Controller parameters from arguments, messages, and dynamic # reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': dict(), 'ki': dict(), 'kd': dict()} self._goal_time = 0.0 self._stopped_velocity = 0.0 self._goal_error = dict() self._path_thresh = dict() # Create our PID controllers self._pid = dict() for joint in self._limb.joint_names(): self._pid[joint] = baxter_control.PID() # Create our spline coefficients self._coeff = [None] * len(self._limb.joint_names()) # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher( '/robot/joint_state_publish_rate', UInt16, queue_size=10) self._pub_rate.publish(self._control_rate) self._pub_ff_cmd = rospy.Publisher( self._ns + '/inverse_dynamics_command', JointTrajectoryPoint, tcp_nodelay=True, queue_size=1) rospy.loginfo("Hi my dudes")
def __init__(self): #Initialize node rospy.init_node('ur_comm_node', anonymous=True) self.VERBOSE = True # Global Vars self.audio_duration = 0 self.finished = False self.devMode = False self.allowCuffInteraction = False self.modeTimer = None # Custom Control Variables --> used in Admittance Control self.control = { 'i': 0, # index we are on out of the 15 values (0-14) 'order': 15, # how many we are keeping for filtering 'effort': [], # Structured self.control['effort'][tap #, e.g. i][joint, e.g. 'right_j0'] 'position': [], 'velocity': [] } # Variables for forces-x --> used in Admittance control, UR specific # Maybe later could include full wrench values? self.forces = { 'i': 0, # index we are on out of the 15 values (0-14) 'order': 15, # how many we are keeping for filtering 'fx': [ ] # different from Sawyer, this gathers the wrench values, forces in x-direction } self.forces['fx'] = [0] * self.forces['order'] self.FORCE_STANDARDIZATION_CONSTANT = .2 # Create empty structures in self.control to match with above comments zeroVec = dict() for joint in JOINT_CONTROL_NAMES: zeroVec[joint] = 0.0 for i in range(self.control['order']): self.control['effort'].append(zeroVec.copy()) self.control['position'].append(zeroVec.copy()) self.control['velocity'].append(zeroVec.copy()) # Publish topics to Browser self.command_complete_topic = rospy.Publisher( '/command_complete', Empty, queue_size=1) #this is for the module/browser self.position_topic = rospy.Publisher('/teachbot/position', JointInfo, queue_size=1) self.velocity_topic = rospy.Publisher('/teachbot/velocity', JointInfo, queue_size=1) self.effort_topic = rospy.Publisher('/teachbot/effort', JointInfo, queue_size=1) # Publish topics to UR self.publish_velocity_to_robot = rospy.Publisher( '/joint_group_vel_controller/command', Float64MultiArray, queue_size=1) # Action Servers self.GoToJointAnglesAct = actionlib.SimpleActionServer( '/teachbot/GoToJointAngles', GoToJointAnglesAction, execute_cb=self.cb_GoToJointAngles, auto_start=True) # Service Servers rospy.Service('/teachbot/audio_duration', AudioDuration, self.rx_audio_duration) rospy.Service('/teachbot/set_robot_mode', SetRobotMode, self.cb_SetRobotMode) # Action Clients - Publish to robot self.joint_traj_client = actionlib.SimpleActionClient( '/scaled_pos_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction) self.velocity_traj_client = actionlib.SimpleActionClient( '/scaled_vel_traj_controller/follow_joint_trajectory', FollowJointTrajectoryAction) # Subscribed Topics rospy.Subscriber('/joint_states', sensor_msgs.msg.JointState, self.forwardJointState) rospy.Subscriber('/wrench', WrenchStamped, self.cb_filter_forces) rospy.loginfo('TeachBot is initialized and ready to go.')
while (time.time() - start_time) < goal.time_to_wait.to_sec(): if server.is_preempt_requested(): result = TimerResult() result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) result.updates_sent = update_count server.set_preempted(result, 'Timer preempted') return feedback = TimerFeedback() feedback.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) feedback.time_remaining = goal.time_to_wait - feedback.time_elapsed server.publish_feedback(feedback) update_count += 1 time.sleep(1.0) result = TimerResult() result.time_elapsed = rospy.Duration.from_sec(time.time() - start_time) result.updates_sent = update_count server.set_succeeded(result, 'Timer completed successfully') print 'starting fancy action server...' rospy.init_node('timer_action_server') server = actionlib.SimpleActionServer('timer', TimerAction, do_timer, False) server.start() rospy.spin() print 'done'
def __init__(self): self.server = actionlib.SimpleActionServer( '/blort_tracker/recognize_object', RecognizeAction, self.execute, False) self.server.start()