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