def __init__(self):
self.heading = None
"""
Setting default values
"""
self.depth = None
self.forward = None
rospy.loginfo("Initializing mission planner interaction module...")
rospy.Subscriber(name=topicHeader.NAV_POSE_ESTIMATED,data_class= krakenPose , callback=self.positionCallback, queue_size=100)
self.setPointController=SimpleActionClient(topicHeader.CONTROL_SETPOINT_ACTION, controllerAction)
rospy.loginfo("waiting for setPoint controller action Server")
self.setPointController.wait_for_server()
rospy.loginfo("Got controller server ")
self.advancedControllerClient=SimpleActionClient(topicHeader.CONTROL_ADVANCEDCONTROLLER_ACTION, advancedControllerAction)
rospy.loginfo("Waiting for advancedController action server")
self.advancedControllerClient.wait_for_server()
rospy.loginfo("Got advanced Controller Action Server ..")
self.ipControllerPublisher=rospy.Publisher(name=topicHeader.CONTROL_IP_ERROR,
data_class=ipControllererror, queue_size=100)
self.moveAlongService=rospy.ServiceProxy(name=topicHeader.CONTROL_MOVEALONG_SERV,
service_class=moveAlongLine)
rospy.loginfo("waiting for MoveAlongLine Service")
self.moveAlongService.wait_for_service()
rospy.loginfo("Got move along line service !!")
self.resetPoseService=rospy.ServiceProxy(name=topicHeader.RESET_POSITION_SERVICE, service_class=krakenResetPose)
rospy.loginfo("waiting for Reset Position Service")
self.resetPoseService.wait_for_service()
rospy.loginfo("Got move reser pose service !!")
self.premapMarkerLocationService=rospy.ServiceProxy(name=topicHeader.PREMAP_LOCATION_SERVICE, service_class=getLocation)
rospy.loginfo("waiting for premap location Service")
self.premapMarkerLocationService.wait_for_service()
rospy.loginfo("Got move premap location service !!")
#also do on and off camera services--To be implemented
############--------------------------
rospy.loginfo("succesfully got all publishers and subsrcibers to mission planner !! ")
def __init__(self, root_link, tip_links):
# tf
self.tfBuffer = Buffer(rospy.Duration(1))
self.tf_listener = TransformListener(self.tfBuffer)
# giskard goal client
# self.client = SimpleActionClient('move', ControllerListAction)
self.client = SimpleActionClient('qp_controller/command',
ControllerListAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer("eef_control")
self.menu_handler = MenuHandler()
for tip_link in tip_links:
int_marker = self.make6DofMarker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root_link, tip_link)
self.server.insert(
int_marker,
self.process_feedback(self.server, self.client, root_link,
tip_link))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
rospy.loginfo("waiting for buoy_detect Server")
buoyClient=SimpleActionClient(header.BUOY_DETECT_ACTION_SERVER, buoyAction)
buoyClient.wait_for_server()
rospy.loginfo("connected to server")
goal=buoyGoal()
goal.order=ip_header.ALLIGN_BUOY
buoyClient.send_goal(goal, feedback_cb= self.feedback_cb)
result=buoyClient.wait_for_result(rospy.Duration(ud.time_out))
if not result:
buoyClient.cancel_goal()
if buoyClient.get_state()==GoalStatus.SUCCEEDED:
rospy.loginfo("successfully aligned the vehicle with the gate ")
return 'aligned'
else:
rospy.logerr("buoy not allogned and timed out before alliging it!!!")
return 'timed_out'
def __init__(self, action_server_name):
# action server
self.client = SimpleActionClient(action_server_name, MoveAction)
self.client.wait_for_server()
self.joint_names = rospy.wait_for_message(
'/whole_body_controller/state',
JointTrajectoryControllerState).joint_names
def execute_cb(self, msg, name, action):
a = action if not self.use_default else self.default_action
self.client = SimpleActionClient(a, MoveBaseAction)
rospy.logdebug("Waiting for action server:" + a)
self.client.wait_for_server()
rospy.logdebug("Sending goal to:" + a)
self.client.send_goal_and_wait(msg)
self.servers[name].set_succeeded()
self.client = None
def test_server_online(self):
"""Check that server is online
move-base is very opaque compared to the move-base-flex framework;
We cannot simply test, if a planner has been loaded successfully.
However, we know that the move_base node crashes and dies, if the
planner is ill-defined.
Hence, we can just test, if the main action is online, to verify, that
the loading-process finished properly.
"""
move_base = SimpleActionClient('/move_base', MoveBaseAction)
self.assertTrue(move_base.wait_for_server(rospy.Duration(60)),
"{} server offline".format(move_base.action_client.ns))
def execute(self, ud):
#comment this at last
#self.resources=Interaction()
#-------------
ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
rospy.loginfo("Waiting for IP marker action server")
ipClient.wait_for_server()
goal = markerGoal()
goal.order = ip_header.DETECT_MARKER
ipClient.cancel_all_goals()
ipClient.send_goal(goal, done_cb=self.doneCb)
result = ipClient.wait_for_result()
if ipClient.get_state() == GoalStatus.SUCCEEDED:
rospy.loginfo("succesfully detected marker :)")
return 'marker_found'
def __init__(self):
rospy.init_node('wilson_ros_planner')
self.client = SimpleActionClient('move_base', MoveBaseAction)
rospy.loginfo("Waiting 5s for move_base action server...")
self.client.wait_for_server(rospy.Duration(5))
rospy.loginfo("Connected to move base server")
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['failed', 'stoped'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('WaitForZone',
WaitForZone(self.zones),
transitions={
'got_zone': 'GotZone',
'waiting_for_zone': 'WaitForZone'
})
smach.StateMachine.add('GotZone',
GotZone(self.zones),
transitions={
'zone_empty': 'WaitForZone',
'got_waypoint': 'GotWaypoint'
})
smach.StateMachine.add('GotWaypoint',
GotWaypoint(self.zones, self.client),
transitions={
'move_to_waypoint': 'GotWaypoint',
'at_waypoint': 'GotZone'
})
# Start Introspection Server
sis = smach_ros.IntrospectionServer('sis', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def __init__(self):
self.measure_dist = rospy.get_param("usv/measurements/dist", 5)
self.width = rospy.get_param("usv/measurements/width", 15)
self.height = rospy.get_param("usv/measurements/height", 15)
self.origin_lat = rospy.get_param("usv/origin/lat", -30.048638)
self.origin_lon = rospy.get_param("usv/origin/lon", -51.23669)
self.start_lat = rospy.get_param("usv/measurements/start/lat",
-30.047311)
self.start_lon = rospy.get_param("usv/measurements/start/lon",
-51.234663)
self.home_lat = rospy.get_param("usv/home/lat", -30.047358)
self.home_lon = rospy.get_param("usv/home/lon", -51.233064)
self.file_name = rospy.get_param("usv/measurements/file_name")
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server()
self.area_pub = rospy.Publisher("/measurement_area",
PolygonStamped,
queue_size=10)
self.point_pub = rospy.Publisher('/measurements',
PointCloud2,
queue_size=10)
self.measurements = []
self.points = []
self.returning_home = False
rospy.Subscriber("/range_depth", Range, self.depth_callback)
rospy.Subscriber("/map", OccupancyGrid, self.map_callback)
rospy.Subscriber("/diffboat/safety", Safety, self.safety_callback)
rospy.Service("next_goal", Empty, self.service_next_goal)
self.has_map = False
self.info = MapMetaData()
self.load()
rospy.on_shutdown(self.save)
def execute(self, ud):
self.resources = Interaction()
#do this job publish data to sensor controller
rospy.loginfo(
"marker detected .....setting vehicle point to marker calling %s",
header.RESET_POSE_SERVICE)
resetClient = rospy.ServiceProxy(header.RESET_POSE_SERVICE,
krakenResetPose)
msg = krakenResetPoseRequest()
resp = resetClient(msg)
##help look here --- http://wiki.ros.org/rospy_tutorials/Tutorials/WritingServiceClient
##################--within this it is done
rospy.loginfo("aligning vehicle to set the marker")
self.ipClient = SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER,
markerAction)
self.ipClient.cancel_all_goals()
goal = markerGoal()
goal.order = header.ALLIGN_MARKER
self.ipClient.send_goal(goal,
done_cb=self.done_cb,
feedback_cb=self.feedback_cb)
self.ipClient.wait_for_result(rospy.Duration(ud.time_out))
if self.result:
rospy.loginfo("successfully aligned the vehicle with the marker ")
#again use the service to reset the vehicle's position
msg = krakenResetPoseRequest()
resp = resetClient(msg)
########################
self.ipClient.cancel_goal()
return 'aligned'
else:
self.ipClient.cancel_goal()
rospy.logerr("marker unaligned and timed out!!!")
return 'timed_out'
def __init__(self):
rospy.init_node('nav_stall', anonymous=True)
rospy.on_shutdown(self.shutdown)
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server()
rospy.loginfo("Connected to move base server")
#initiate bucket counter variables
self.stallCounterBucket = 0
self.STOP_MAX_STALL_BUCKET_COUNT = 10
self.MAX_STALL_BUCKET_COUNT = 3
self.hasGivenUp = 0
self.current_topic = rospy.get_param("~current_topic")
self.stall_current = rospy.get_param("~stall_current", 0.1)
self.recovery_speed = rospy.get_param("~recovery_speed", 0.1)
self.recovery_time = rospy.get_param("~recovery_time", 2)
rospy.loginfo("Time: " + str(self.recovery_time))
# Wait for motor current topics to become available
rospy.loginfo("Waiting for motor current topic to become available...")
rospy.wait_for_message(self.current_topic, AnalogFloat)
#subscribe to motor current topics
rospy.Subscriber(self.current_topic, AnalogFloat, self.detect_stall)
rospy.loginfo("Stall detection started on " + self.current_topic)
def start_once(self):
# giskard goal client
self.client = SimpleActionClient(u'qp_controller/command', MoveAction)
self.client.wait_for_server()
# marker server
self.server = InteractiveMarkerServer(u'eef_control{}'.format(
self.suffix))
self.menu_handler = MenuHandler()
all_goals = {}
for root, tip in zip(self.roots, self.tips):
int_marker = self.make_6dof_marker(
InteractiveMarkerControl.MOVE_ROTATE_3D, root, tip)
self.server.insert(
int_marker,
self.process_feedback(self.server, int_marker.name,
self.client, root, tip, all_goals))
self.menu_handler.apply(self.server, int_marker.name)
self.server.applyChanges()
def test_server_online(self):
"""Check that server is online
In this simple setup, we don't really want to generate a plan.
Instread, we can ask for a plan on our planner (gpp_gp), and verify,
that the generated error-message is not INVALID_PLUGIN
See https://github.com/magazino/move_base_flex/blob/596ed881bfcbd847e9d296c6d38e4d3fa3b74a4d/mbf_msgs/action/GetPath.action
for reference.
"""
# setup the client
get_path = SimpleActionClient('move_base_flex/get_path', GetPathAction)
self.assertTrue(get_path.wait_for_server(rospy.Duration(60)),
"{} server offline".format(get_path.action_client.ns))
# send a dummy goal with the right planner
goal = GetPathGoal()
goal.planner = 'gpp_gp'
get_path.send_goal_and_wait(goal, rospy.Duration(1))
result = get_path.get_result()
# we are happy as long the plugin is known
self.assertNotEqual(result.outcome, GetPathResult.INVALID_PLUGIN)
def __init__(self, hebi_group_name, hebi_mapping, leg_base_links,
leg_end_links):
rospy.loginfo("Creating Hexapod instance...")
hebi_families = []
hebi_names = []
for leg in hebi_mapping:
for actuator in leg:
family, name = actuator.split('/')
hebi_families.append(family)
hebi_names.append(name)
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_mapping = hebi_mapping
self.hebi_mapping_flat = self._flatten(self.hebi_mapping)
# jt information populated by self._feedback_cb
self._current_jt_pos = {}
self._current_jt_vel = {}
self._current_jt_eff = {}
self._joint_state_pub = None
self._hold_leg_list = [False, False, False, False, False, False]
self._hold_leg_positions = self._get_list_of_lists()
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(
" Waiting for SendCommandWithAcknowledgementSrv at %s ...",
"/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/" + hebi_group_name +
"/send_command_with_acknowledgement"
) # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = self.hebi_mapping_flat
cmd_msg.settings.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.name = self.hebi_mapping_flat
cmd_msg.settings.position_gains.kp = [5, 8, 2] * LEGS
cmd_msg.settings.position_gains.ki = [0.001] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.kd = [0] * LEGS * ACTUATORS_PER_LEG
cmd_msg.settings.position_gains.i_clamp = [
0.25
] * LEGS * ACTUATORS_PER_LEG # TODO: Tune this. Setting it low for testing w/o restarting Gazebo
change_group_settings(cmd_msg)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
self._hold_position = False
self._hold_joint_states = {}
# TrajectoryAction client
self.trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
self.trajectory_action_client.wait_for_server(
) # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Twist Subscriber
self._cmd_vel_sub = rospy.Subscriber("/hexapod/cmd_vel/", Twist,
self._cmd_vel_cb)
self.last_vel_cmd = None
self.linear_displacement_limit = 0.075 # m
self.angular_displacement_limit = 0.65 # rad
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
while not rospy.is_shutdown() and not urdf_loaded:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.01) # sleep for 10 ms of ROS time
# pykdl_utils setup
self.robot_urdf = URDF.from_xml_string(urdf_str)
self.kdl_fk_base_to_leg_base = [
KDLKinematics(self.robot_urdf, 'base_link', base_link)
for base_link in leg_base_links
]
self.kdl_fk_leg_base_to_eff = [
KDLKinematics(self.robot_urdf, base_link, end_link)
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
# trac-ik setup
self.trac_ik_leg_base_to_end = [
IK(
base_link,
end_link,
urdf_string=urdf_str,
timeout=0.01, # TODO: Tune me
epsilon=1e-4,
solve_type="Distance")
for base_link, end_link in zip(leg_base_links, leg_end_links)
]
self.ik_pos_xyz_bounds = [0.01, 0.01, 0.01]
self.ik_pos_wxyz_bounds = [31416.0, 31416.0, 31416.0
] # NOTE: This implements position-only IK
# Wait for connections to be set up
rospy.loginfo("Wait for ROS connections to be set up...")
while not rospy.is_shutdown() and len(self._current_jt_pos) < len(
self.hebi_mapping_flat):
rospy.sleep(0.1)
# Set up joint state publisher
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._loop_rate = rospy.Rate(20)
# leg joint home pos Hip, Knee, Ankle
self.leg_jt_home_pos = [
[0.0, +0.26, -1.57], # Leg 1
[0.0, -0.26, +1.57], # Leg 2
[0.0, +0.26, -1.57], # Leg 3
[0.0, -0.26, +1.57], # Leg 4
[0.0, +0.26, -1.57], # Leg 5
[0.0, -0.26, +1.57]
] # Leg 6
# leg end-effector home position
self.leg_eff_home_pos = self._get_leg_base_to_eff_fk(
self.leg_jt_home_pos)
# leg step height relative to leg base link
self.leg_eff_step_height = [[]] * LEGS # relative to leg base
for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base):
base_to_leg_base_rot = fk_solver.forward([])[:3, :3]
step_ht_chassis = np.array([0, 0, STEP_HEIGHT])
step_ht_leg_base = np.dot(base_to_leg_base_rot, step_ht_chassis)
self.leg_eff_step_height[i] = step_ht_leg_base.tolist()[0]
self._odd_starts = True
rospy.loginfo("Done creating Hexapod instance...")
def main():
# Initialize ROS node
rospy.init_node("example_04_trajectory_node", disable_signals=True)
rate = rospy.Rate(200)
group_name = "my_group"
num_joints = 3
num_waypoints = 5
# Create a client which uses the service to create a group
add_group_client = rospy.ServiceProxy("hebiros/add_group_from_names", AddGroupFromNamesSrv)
# Create a subscriber to receive feedback from a group
# Register feedback callback which runs when feedback is received
feedback_subscriber = rospy.Subscriber("/hebiros/"+group_name+"/feedback/joint_state", JointState, feedback_callback, queue_size=100)
# Construct a group using 3 known modules
rospy.wait_for_service("hebiros/add_group_from_names")
req_group_name = group_name
req_names = ["base", "shoulder", "elbow"]
req_families = ["HEBI"]
add_group_client(req_group_name, req_names, req_families)
# Create an action client for executing a trajectory
client = SimpleActionClient("/hebiros/"+group_name+"/trajectory", TrajectoryAction)
# Wait for the action server corresponding to the action client
client.wait_for_server()
user_input = raw_input("Press Enter/Return to execute Trajectory")
# Construct a trajectory to be sent as an action goal
goal = TrajectoryGoal()
# Set the times to reach each waypoint in seconds
times = [0, 5, 10, 15, 20]
names = ["HEBI/base", "HEBI/shoulder", "HEBI/elbow"]
# Wait for feedback from actuators
while not rospy.is_shutdown() and feedback.position is not None and len(feedback.position) < len(names):
rate.sleep()
# Set positions, velocities, and accelerations for each waypoint and each joint
# The following vectors have one joint per row and one waypoint per column
positions = [[feedback.position[0], 0, M_PI_2, 0, 0],
[feedback.position[1], 0, M_PI_2, M_PI_2, 0],
[feedback.position[2], 0, 0, M_PI_2, 0]]
velocities = [[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0]]
accelerations = [[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0],
[0, NAN, NAN, NAN, 0]]
# Construct the goal using the TrajectoryGoal format
for i in range(num_waypoints):
waypoint = WaypointMsg()
waypoint.names = names
waypoint.positions = [joint[i] for joint in positions]
waypoint.velocities = [joint[i] for joint in velocities]
waypoint.accelerations = [joint[i] for joint in accelerations]
goal.waypoints.append(waypoint)
goal.times.append(times[i])
# Send the goal, executing the trajectory
client.send_goal(goal, trajectory_done, trajectory_active, trajectory_feedback)
while not rospy.is_shutdown():
rate.sleep()
def __init__(self, test):
self.test = test
#print "recorder_core: self.test.name:", self.test.name
recorder_config = self.load_data(
rospkg.RosPack().get_path("atf_recorder_plugins") +
"/config/recorder_plugins.yaml")
try:
# delete test_results directories and create new ones
if os.path.exists(self.test.generation_config["bagfile_output"]):
# shutil.rmtree(self.tests.generation_config"bagfile_output"]) #FIXME will fail if multiple test run concurrently
pass
else:
os.makedirs(self.test.generation_config["bagfile_output"])
except OSError:
pass
# lock for self variables of this class
self.lock = Lock()
# create bag file writer handle
self.lock_write = Lock()
self.bag = rosbag.Bag(
self.test.generation_config["bagfile_output"] + self.test.name +
".bag", 'w')
self.bag_file_writer = BagfileWriter(self.bag, self.lock_write)
# Init metric recorder
self.recorder_plugin_list = []
#print "recorder_config", recorder_config
if len(recorder_config) > 0:
for value in list(recorder_config.values()):
#print "value=", value
self.recorder_plugin_list.append(
getattr(atf_recorder_plugins, value)(self.lock_write,
self.bag_file_writer))
#print "self.recorder_plugin_list", self.recorder_plugin_list
#rospy.Service(self.topic + "recorder_command", RecorderCommand, self.command_callback)
self.diagnostics = None
rospy.Subscriber("/diagnostics_toplevel_state", DiagnosticStatus,
self.diagnostics_callback)
rospy.on_shutdown(self.shutdown)
# wait for topics, services, actions and tfs to become active
if test.robot_config != None and 'wait_for_topics' in test.robot_config:
for topic in test.robot_config["wait_for_topics"]:
rospy.loginfo("Waiting for topic '%s'...", topic)
rospy.wait_for_message(topic, rospy.AnyMsg)
rospy.loginfo("... got message on topic '%s'.", topic)
if test.robot_config != None and 'wait_for_services' in test.robot_config:
for service in test.robot_config["wait_for_services"]:
rospy.loginfo("Waiting for service '%s'...", service)
rospy.wait_for_service(service)
rospy.loginfo("... service '%s' available.", service)
if test.robot_config != None and 'wait_for_actions' in test.robot_config:
for action in test.robot_config["wait_for_actions"]:
rospy.loginfo("Waiting for action '%s'...", action)
# wait for action status topic
rospy.wait_for_message(action + "/status", rospy.AnyMsg)
# get action type of goal topic
topic_type = rostopic._get_topic_type(action + "/goal")[0]
# remove "Goal" string from action type
if topic_type == None or not "Goal" in topic_type: ## pylint: disable=unsupported-membership-test
msg = "Could not get type for action %s. type is %s" % (
action, topic_type)
rospy.logerr(msg)
raise ATFRecorderError(msg)
# remove "Goal" from type
topic_type = topic_type[0:len(topic_type) - 4] ## pylint: disable=unsubscriptable-object
client = SimpleActionClient(
action, roslib.message.get_message_class(topic_type))
# wait for action server
client.wait_for_server()
rospy.loginfo("... action '%s' available.", action)
if test.robot_config != None and 'wait_for_tfs' in test.robot_config:
listener = tf.TransformListener()
for root_frame, measured_frame in test.robot_config[
"wait_for_tfs"]:
rospy.loginfo(
"Waiting for transformation from '%s' to '%s' ...",
root_frame, measured_frame)
listener.waitForTransform(root_frame, measured_frame,
rospy.Time(), rospy.Duration(1))
rospy.loginfo(
"... transformation from '%s' to '%s' available.",
root_frame, measured_frame)
if test.robot_config != None and 'wait_for_diagnostics' in test.robot_config and test.robot_config[
"wait_for_diagnostics"]:
rospy.loginfo("Waiting for diagnostics to become OK ...")
r = rospy.Rate(100)
while not rospy.is_shutdown(
) and self.diagnostics != None and self.diagnostics.level != 0:
rospy.logdebug("... waiting for diagnostics to become OK ...")
r.sleep()
rospy.loginfo("... diagnostics are OK.")
self.active_topics = {}
# special case for tf: make sure tf_buffer is already filled (even before the testblocks are started). Thus we need to record /tf and /tf_static all the time, even if there is no active testblock using tf.
for testblock in self.test.testblocks:
topics = self.get_topics_of_testblock(testblock.name)
if "/tf" in topics or "/tf_static" in topics:
self.active_topics["/tf"] = ["always"]
self.active_topics["/tf_static"] = ["always"]
self.subscribers = {}
self.tf_static_message = TFMessage()
rospy.Timer(rospy.Duration(0.1), self.create_subscriber_callback)
rospy.Timer(rospy.Duration(0.1), self.tf_static_timer_callback)
rospy.loginfo("ATF recorder: started!")
def __init__(self, action_server_name):
# action server
self.client = SimpleActionClient(action_server_name,
ControllerListAction)
self.client.wait_for_server()
def __init__(self):
rospy.init_node('nav_test', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 10)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['hall'] = Pose(Point(-0.486, -0.689, 0.000),
Quaternion(0.000, 0.000, 0.000, 1.000))
locations['living_room'] = Pose(Point(1.623, 7.880, 0.000),
Quaternion(0.000, 0.000, 0.707, 0.707))
locations['kitchen'] = Pose(Point(-1.577, 6.626, 0.000),
Quaternion(0.000, 0.000, 1.000, 0.000))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)
# Subscribe to the move_base action server
self.move_base = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = n_locations
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
# Get the initial pose from the user
rospy.loginfo(
"*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose..."
)
rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped,
self.update_initial_pose)
# Make sure we have the initial pose
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.loginfo("Starting navigation test")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the current sequence,
# start with a new random sequence
if i == n_locations:
i = 0
sequence = sample(locations, n_locations)
# Skip over first location if it is the same as
# the last location
if sequence[0] == last_location:
i = 1
# Get the next location in the current sequence
location = sequence[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(
pow(
locations[location].position.x -
locations[last_location].position.x, 2) + pow(
locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(
pow(
locations[location].position.x -
initial_pose.pose.pose.position.x, 2) + pow(
locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal)
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(
rospy.Duration(300))
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
else:
rospy.loginfo("Goal failed with error code: " +
str(goal_states[state]))
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes / n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " +
str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names', AddGroupFromNamesSrv)
# Topic to receive feedback from a group
hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s", "/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s", "/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...", '/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names') # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
trajectory_action_client = SimpleActionClient("/hebiros/"+hebi_group_name+"/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...", "/hebiros/"+hebi_group_name+"/trajectory")
trajectory_action_client.wait_for_server() # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Create a service client to set group settings
change_group_settings = rospy.ServiceProxy("/hebiros/"+hebi_group_name+"/send_command_with_acknowledgement",
SendCommandWithAcknowledgementSrv)
rospy.loginfo(" Waiting for SendCommandWithAcknowledgementSrv at %s ...", "/hebiros/"+hebi_group_name+"/send_command_with_acknowledgement")
rospy.wait_for_service("/hebiros/"+hebi_group_name+"/send_command_with_acknowledgement") # block until service server starts
cmd_msg = CommandMsg()
cmd_msg.name = hebi_mapping_flat
cmd_msg.settings.name = cmd_msg.name
cmd_msg.settings.velocity_gains.name = cmd_msg.name
cmd_msg.settings.position_gains.kp = [0.5, 1.5, 1.5, 0.5, 1.5, 1.5]
cmd_msg.settings.position_gains.ki = [0]*6
def __init__(
self,
# Action info
action_name,
action_spec,
# Default goal
goal=None,
goal_key=None,
goal_slots=[],
goal_cb=None,
goal_cb_args=[],
goal_cb_kwargs={},
# Result modes
result_key=None,
result_slots=[],
result_cb=None,
result_cb_args=[],
result_cb_kwargs={},
# Keys
input_keys=[],
output_keys=[],
outcomes=[],
# Timeouts
exec_timeout=None,
cancel_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(60.0),
):
"""Constructor for SimpleActionState action client wrapper.
@type action_name: string
@param action_name: The name of the action as it will be broadcast over ros.
@type action_spec: actionlib action msg
@param action_spec: The type of action to which this client will connect.
@type goal: actionlib goal msg
@param goal: If the goal for this action does not need to be generated at
runtime, it can be passed to this state on construction.
@type goal_key: string
@param goal_key: Pull the goal message from a key in the userdata.
This will be done before calling the goal_cb if goal_cb is defined.
@type goal_slots: list of string
@param goal_slots: Pull the goal fields (__slots__) from like-named
keys in userdata. This will be done before calling the goal_cb if
goal_cb is defined.
@type goal_cb: callable
@param goal_cb: If the goal for this action needs to be generated at
runtime, a callback can be stored which modifies the default goal
object. The callback is passed two parameters:
- userdata
- current stored goal
The callback returns a goal message.
@type result_key: string
@param result_key: Put the result message into the userdata with
the given key. This will be done after calling the result_cb
if result_cb is defined.
@type result_slots: list of strings
@param result_slots: Put the result message fields (__slots__)
into the userdata with keys like the field names. This will be done
after calling the result_cb if result_cb is defined.
@type result_cb: callable
@param result_cb: If result information from this action needs to be
stored or manipulated on reception of a result from this action, a
callback can be stored which is passed this information. The callback
is passed three parameters:
- userdata (L{UserData<smach.user_data.UserData>})
- result status (C{actionlib.GoalStatus})
- result (actionlib result msg)
@type exec_timeout: C{rospy.Duration}
@param exec_timeout: This is the timeout used for sending a preempt message
to the delegate action. This is C{None} by default, which implies no
timeout.
@type cancel_timeout: C{rospy.Duration}
@param cancel_timeout: This is the timeout used for aborting the state after a
preempt has been requested or the execution timeout occured and no result
from the action has been received. This timeout begins counting after cancel
to the action server has been sent.
@type server_wait_timeout: C{rospy.Duration}
@param server_wait_timeout: This is the timeout used for aborting while
waiting for an action server to become active.
"""
# Initialize base class
State.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'])
# Set action properties
self._action_name = action_name
self._action_spec = action_spec
# Set timeouts
self._goal_status = 0
self._goal_result = None
self._exec_timeout = exec_timeout
self._cancel_timeout = cancel_timeout
self._server_wait_timeout = server_wait_timeout
# Set goal generation policy
if goal and hasattr(goal, '__call__'):
raise smach.InvalidStateError(
"Goal object given to SimpleActionState that IS a function object"
)
sl = action_spec().action_goal.goal.__slots__
if not all([s in sl for s in goal_slots]):
raise smach.InvalidStateError(
"Goal slots specified are not valid slots. Available slots: %s; specified slots: %s"
% (sl, goal_slots))
if goal_cb and not hasattr(goal_cb, '__call__'):
raise smach.InvalidStateError(
"Goal callback object given to SimpleActionState that IS NOT a function object"
)
# Static goal
if goal is None:
self._goal = copy.copy(action_spec().action_goal.goal)
else:
self._goal = goal
# Goal from userdata key
self._goal_key = goal_key
if goal_key is not None:
self.register_input_keys([goal_key])
# Goal slots from userdata keys
self._goal_slots = goal_slots
self.register_input_keys(goal_slots)
# Goal generation callback
self._goal_cb = goal_cb
self._goal_cb_args = goal_cb_args
self._goal_cb_kwargs = goal_cb_kwargs
if smach.has_smach_interface(goal_cb):
self._goal_cb_input_keys = goal_cb.get_registered_input_keys()
self._goal_cb_output_keys = goal_cb.get_registered_output_keys()
self.register_input_keys(self._goal_cb_input_keys)
self.register_output_keys(self._goal_cb_output_keys)
else:
self._goal_cb_input_keys = input_keys
self._goal_cb_output_keys = output_keys
# Set result processing policy
if result_cb and not hasattr(result_cb, '__call__'):
raise smach.InvalidStateError(
"Result callback object given to SimpleActionState that IS NOT a function object"
)
if not all([
s in action_spec().action_result.result.__slots__
for s in result_slots
]):
raise smach.InvalidStateError(
"Result slots specified are not valid slots.")
# Result callback
self._result_cb = result_cb
self._result_cb_args = result_cb_args
self._result_cb_kwargs = result_cb_kwargs
if smach.has_smach_interface(result_cb):
self._result_cb_input_keys = result_cb.get_registered_input_keys()
self._result_cb_output_keys = result_cb.get_registered_output_keys(
)
self._result_cb_outcomes = result_cb.get_registered_outcomes()
self.register_input_keys(self._result_cb_input_keys)
self.register_output_keys(self._result_cb_output_keys)
self.register_outcomes(self._result_cb_outcomes)
else:
self._result_cb_input_keys = input_keys
self._result_cb_output_keys = output_keys
self._result_cb_outcomes = outcomes
# Result to userdata key
self._result_key = result_key
if result_key is not None:
self.register_output_keys([result_key])
# Result slots to userdata keys
self._result_slots = result_slots
self.register_output_keys(result_slots)
# Register additional input and output keys
self.register_input_keys(input_keys)
self.register_output_keys(output_keys)
self.register_outcomes(outcomes)
# Declare some status variables
self._activate_time = rospy.Time.now()
self._cancel_time = rospy.Time.now()
self._duration = rospy.Duration(0.0)
self._status = SimpleActionState.WAITING_FOR_SERVER
# Construct action client, and wait for it to come active
self._action_client = SimpleActionClient(action_name, action_spec)
self._action_wait_thread = threading.Thread(
name=self._action_name + '/wait_for_server',
target=self._wait_for_server)
self._action_wait_thread.start()
self._execution_timer_thread = None
self._cancelation_timer_thread = None
# Condition variables for threading synchronization
self._done_cond = threading.Condition()
def __init__(self, hebi_group_name, hebi_families, hebi_names):
rospy.loginfo("Creating TrajectoryRecorder instance...")
rospy.loginfo(" hebi_group_name: %s", hebi_group_name)
rospy.loginfo(" hebi_families: %s", hebi_families)
rospy.loginfo(" hebi_names: %s", hebi_names)
self.hebi_group_name = hebi_group_name
self.hebi_families = hebi_families
self.hebi_names = hebi_names
self.hebi_mapping = [
family + '/' + name
for family, name in zip(hebi_families, hebi_names)
]
# jt information populated by self._feedback_cb
self.current_jt_pos = {}
self.current_jt_vel = {}
self.current_jt_eff = {}
self._joint_state_pub = None
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Create a service client to set the command lifetime
self.set_command_lifetime = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name + "/set_command_lifetime",
SetCommandLifetimeSrv)
# Topic to receive feedback from a group
self.hebi_group_feedback_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
self.hebi_group_command_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names'
) # block until service server starts
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# Feedback/Command
self.fbk_sub = rospy.Subscriber(self.hebi_group_feedback_topic,
JointState, self._feedback_cb)
self.cmd_pub = rospy.Publisher(self.hebi_group_command_topic,
JointState,
queue_size=1)
self._hold_position = False
self._hold_joint_states = []
# TrajectoryAction client
self.trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
self.trajectory_action_client.wait_for_server(
) # block until action server starts
self._executing_trajectory = False
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
# Check ROS Parameter server for robot_description URDF
urdf_str = ""
urdf_loaded = False
time_end_check = rospy.Time.now().to_sec(
) + 2.0 # Stop looking for URDF after 2 seconds of ROS time
while not rospy.is_shutdown(
) and not urdf_loaded and rospy.Time.now().to_sec() < time_end_check:
if rospy.has_param('/robot_description'):
urdf_str = rospy.get_param('/robot_description')
urdf_loaded = True
rospy.loginfo(
"Pulled /robot_description from parameter server.")
else:
rospy.sleep(0.05) # sleep for 50 ms of ROS time
if urdf_loaded:
# pykdl_utils setup
self.robot_urdf = URDF.from_xml_string(urdf_str)
rospy.loginfo("URDF links: " +
str([link.name
for link in self.robot_urdf.links])[1:-1])
rospy.loginfo("URDF joints: " +
str([joint.name
for joint in self.robot_urdf.joints])[1:-1])
valid_names = False
while not rospy.is_shutdown() and not valid_names:
# self.chain_base_link_name = self.raw_input_ros_safe("Please enter kinematics chain's base link name\n") # FIXME: TEMP
# self.chain_end_link_name = self.raw_input_ros_safe("Please enter kinematics chain's end link name\n") # FIXME: TEMP
self.chain_base_link_name = "a_2043_01_5Z" # FIXME: TEMP
self.chain_end_link_name = "a_2039_02_4Z" # FIXME: TEMP
try:
self.kdl_kin = KDLKinematics(self.robot_urdf,
self.chain_base_link_name,
self.chain_end_link_name)
valid_names = True
except KeyError:
rospy.loginfo(
"Incorrect base or end link name. Please try again...")
# trac-ik setup
ik_solver = IK(self.chain_base_link_name,
self.chain_end_link_name,
urdf_string=urdf_str,
timeout=0.01,
epsilon=1e-4,
solve_type="Distance")
# Determine transformation from global reference frame to base_link
urdf_root = ET.fromstring(urdf_str)
cadassembly_metrics = urdf_root.find(
'link/CyPhy2CAD/CADAssembly_metrics')
if cadassembly_metrics is not None:
robot_base_link_transform = np.zeros((4, 4))
robot_base_link_transform[3, 3] = 1
rotation_matrix_elem = cadassembly_metrics.find(
'RotationMatrix')
for j, row in enumerate(rotation_matrix_elem.iter('Row')):
for i, column in enumerate(row.iter('Column')):
robot_base_link_transform[j, i] = column.get('Value')
translation_elem = cadassembly_metrics.find('Translation')
for j, attribute in enumerate(['X', 'Y', 'Z']):
robot_base_link_transform[j, 3] = translation_elem.get(
attribute)
kdl_kin_robot_base_link_to_chain_base_link = KDLKinematics(
self.robot_urdf, 'base_link', self.chain_base_link_name)
jt_angles = [
0.0
] * kdl_kin_robot_base_link_to_chain_base_link.num_joints
chain_base_link_transform = kdl_kin_robot_base_link_to_chain_base_link.forward(
jt_angles)
self.chain_base_link_abs_transform = np.dot(
robot_base_link_transform, chain_base_link_transform)
else:
self.chain_base_link_abs_transform = None
# Wait for connections to be setup
while not rospy.is_shutdown() and len(self.current_jt_pos) < len(
self.hebi_mapping):
rospy.sleep(0.1)
# Set up joint state publisher
self._joint_state_pub = rospy.Publisher('/joint_states',
JointState,
queue_size=1)
self._active_joints = self.kdl_kin.get_joint_names()
# Set up RViz Interactive Markers
self.int_markers_man = InteractiveMarkerManager(
self,
'trajectory_recording_tool/interactive_markers',
ik_solver=ik_solver)
else:
self.robot_urdf = None
self.waypoints = []
self.waypoints_cnt = 0
self.breakpoints_cnt = 0
self._prev_breakpoint = True
# rospkg
self._rospack = RosPack()
print()
def main():
## Parse args
parser = parse_args(sys.argv[1:])
hebi_group_name = parser.hebi_group_name
hebi_families = [
parser.hebi_base_family, parser.hebi_shoulder_family,
parser.hebi_elbow_family
]
hebi_names = [
parser.hebi_base_name, parser.hebi_shoulder_name,
parser.hebi_elbow_name
]
from_master_topic = parser.from_master_topic
to_master_topic = parser.to_master_topic
# ROS stuff
rospy.init_node('smach_fsm_using_execute_joint_trajectory_state',
anonymous=True)
rate = rospy.Rate(200)
# HEBI setup - NOTE: hebiros_node must be running
rospy.loginfo("HEBI Group name: " + str(hebi_group_name))
rospy.loginfo("HEBI families: " + str(hebi_families))
rospy.loginfo("HEBI names: " + str(hebi_names))
hebi_mapping = [
family + '/' + name for family, name in zip(hebi_families, hebi_names)
]
# Create a service client to create a group
set_hebi_group = rospy.ServiceProxy('/hebiros/add_group_from_names',
AddGroupFromNamesSrv)
# Create a service client to set the command lifetime
set_command_lifetime = rospy.ServiceProxy(
"/hebiros/" + hebi_group_name + "/set_command_lifetime",
SetCommandLifetimeSrv)
# Topic to receive feedback from a group
hebi_group_fbk_topic = "/hebiros/" + hebi_group_name + "/feedback/joint_state"
rospy.loginfo(" hebi_group_feedback_topic: %s",
"/hebiros/" + hebi_group_name + "/feedback/joint_state")
# Topic to send commands to a group
hebi_group_cmd_topic = "/hebiros/" + hebi_group_name + "/command/joint_state"
rospy.loginfo(" hebi_group_command_topic: %s",
"/hebiros/" + hebi_group_name + "/command/joint_state")
# Call the /hebiros/add_group_from_names service to create a group
rospy.loginfo(" Waiting for AddGroupFromNamesSrv at %s ...",
'/hebiros/add_group_from_names')
rospy.wait_for_service('/hebiros/add_group_from_names')
rospy.loginfo(" AddGroupFromNamesSrv AVAILABLE.")
set_hebi_group(hebi_group_name, hebi_names, hebi_families)
# TrajectoryAction client
trajectory_action_client = SimpleActionClient(
"/hebiros/" + hebi_group_name + "/trajectory", TrajectoryAction)
rospy.loginfo(" Waiting for TrajectoryActionServer at %s ...",
"/hebiros/" + hebi_group_name + "/trajectory")
trajectory_action_client.wait_for_server(
) # block until action server starts
rospy.loginfo(" TrajectoryActionServer AVAILABLE.")
### CREATE ARM STATE INSTANCES ###
# trace line
trace_line = ExecuteJointTrajectoryFromFile("hebi_3dof_arm_description",
"trace_line_0.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=3.0)
# stir pot
stir_pot_setup = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stir_pot_0.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=2.0)
stir_pot_loop_1 = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stir_pot_1.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=1.0)
stir_pot_loop_n = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stir_pot_1.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=0.0)
break_stir_pot_loop = BreakOnMsg("break_topic_1")
stir_pot_exit = ExecuteJointTrajectoryFromFile("hebi_3dof_arm_description",
"stir_pot_2.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=1.0)
# stab bag of veggies
stab_bag_setup = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stab_bag_0.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=3.0)
stab_bag_loop_n = ExecuteJointTrajectoryFromFile(
"hebi_3dof_arm_description",
"stab_bag_1.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=0.5)
break_stab_bag_loop = BreakOnMsg("break_topic_2")
stab_bag_exit = ExecuteJointTrajectoryFromFile("hebi_3dof_arm_description",
"stab_bag_2.json",
hebi_mapping,
trajectory_action_client,
set_command_lifetime,
hebi_group_fbk_topic,
hebi_group_cmd_topic,
setup_time=1.5)
### CREATE TOP SM ###
top = StateMachine(outcomes=['exit', 'success'])
### INITIALIZE USERDATA ###
# Updated by ExecuteJointTrajectoryFromFile State instances
top.userdata.final_joint_positions = [None, None, None]
# Keeps things a tad neater
remapping_dict = {'final_joint_positions': 'final_joint_positions'}
with top:
### ADD LEG STATES TO THE TOP SM ###
StateMachine.add('TRACE_LINE',
trace_line,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_SETUP',
'success': 'STIR_POT_SETUP'
},
remapping=remapping_dict)
StateMachine.add('STIR_POT_SETUP',
stir_pot_setup,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_SETUP',
'success': 'STIR_POT_LOOP_1'
},
remapping=remapping_dict)
StateMachine.add('STIR_POT_LOOP_1',
stir_pot_loop_1,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_LOOP_N',
'success': 'STIR_POT_LOOP_N'
},
remapping=remapping_dict)
StateMachine.add('STIR_POT_LOOP_N',
stir_pot_loop_n,
transitions={
'exit': 'exit',
'failure': 'STIR_POT_LOOP_N',
'success': 'BREAK_STIR_POT_LOOP'
},
remapping=remapping_dict)
StateMachine.add('BREAK_STIR_POT_LOOP',
break_stir_pot_loop,
transitions={
'exit': 'exit',
'true': 'STIR_POT_EXIT',
'false': 'STIR_POT_LOOP_N'
})
StateMachine.add('STIR_POT_EXIT',
stir_pot_exit,
transitions={
'exit': 'exit',
'failure': 'exit',
'success': 'STAB_BAG_SETUP'
},
remapping=remapping_dict)
StateMachine.add('STAB_BAG_SETUP',
stab_bag_setup,
transitions={
'exit': 'exit',
'failure': 'STAB_BAG_SETUP',
'success': 'STAB_BAG_LOOP_N'
},
remapping=remapping_dict)
StateMachine.add('STAB_BAG_LOOP_N',
stab_bag_loop_n,
transitions={
'exit': 'exit',
'failure': 'STAB_BAG_LOOP_N',
'success': 'BREAK_STAB_BAG_LOOP'
},
remapping=remapping_dict)
StateMachine.add('BREAK_STAB_BAG_LOOP',
break_stab_bag_loop,
transitions={
'exit': 'exit',
'true': 'STAB_BAG_EXIT',
'false': 'STAB_BAG_LOOP_N'
})
StateMachine.add('STAB_BAG_EXIT',
stir_pot_exit,
transitions={
'exit': 'exit',
'failure': 'exit',
'success': 'success'
},
remapping=remapping_dict)
sis = smach_ros.IntrospectionServer(str(rospy.get_name()), top,
'/SM_ROOT' + str(rospy.get_name()))
sis.start()
user_input = raw_input(
"Please press the 'Return/Enter' key to start executing \
- pkg: smach_fsm_using_execute_joint_trajectory_state.py | node: " +
str(rospy.get_name()) + "\n")
print("Input received. Executing "
"- pkg: smach_fsm_using_execute_joint_trajectory_state | node: " +
str(rospy.get_name()) + "\n")
top.execute()
rospy.spin()
sis.stop()
print("\nExiting " + str(rospy.get_name()))
# TODO: REMOVE such *** code
from moveit_msgs.msg import MoveItErrorCodes, MoveGroupAction
from actionlib.simple_action_client import SimpleActionClient
from moveit_msgs.srv import GetPositionIK, GetPositionIKRequest, GetPositionFKRequest, GetPositionFK
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
import rospy
from moveit_builder import MoveItGoalBuilder
builder = MoveItGoalBuilder()
action_client = SimpleActionClient('move_group', MoveGroupAction)
sim_pub = rospy.Publisher('/arm_controller/command',
JointTrajectory,
queue_size=10)
joint_names = [
'joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'
]
link_names = ["link_1", "link_2", "link_3", "link_4", "link_5", "link_6"]
def get_pose(): # TODO: test me
js = rospy.wait_for_message("/joint_states", JointState)
pose = solve_fk(js)
return pose
def execute_pose(pose, planning_time=1):
positions = solve_ik(pose)
if positions != None:
# simulation
