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 __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'
class AligningVehicle(smach.State): def __init__(self,resources): smach.State.__init__(self, outcomes=['aligned','timed_out'],input_keys=['time_out','e_x','e_y']) self.resources=resources 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 feedback_cb(self,feed): ##publish this error data to the sensor and move the vehicle msg=ipControllererror() msg.dy=feed.errorx msg.dx=msg.dy/tan(feed.errorangle) self.resources.ipControllerPublisher.publish(msg) ##done moving def done_cb(self,result): if result.sequence==header.MARKER_ALLIGNED: self.result=True else: self.result=False
class TravelState(StateMachine): def __init__(self): StateMachine.__init__(self, input_keys=['target_pose'], outcomes=['succeeded', 'preempted', 'aborted'] ) self.sm_move = Concurrence( input_keys=['target_pose'], output_keys=['ball_pose'], outcomes=['succeeded', 'preempted', 'aborted'], default_outcome='aborted', child_termination_cb = self.child_termination_cb, outcome_cb = self.outcome_cb ) self.action_client = SimpleActionClient('move_base', MoveBaseAction) #self.action_client = PretendActionClient() self.move_state = Move(self.action_client, 0.2) self.chaser_state = BallChaser(self.action_client, 0.2) self.userdata.msg_in = None self.watcher = BallTracker(self.move_state, self.chaser_state) with self.sm_move: Concurrence.add('BALL_WATCHER', self.watcher) Concurrence.add('MOVE_TO_GOAL', self.move_state) with self: StateMachine.add('SM_MOVE', self.sm_move) StateMachine.add('CHASE_BALLS', self.move_state, transitions={ 'succeeded':'SM_MOVE', 'aborted':'SM_MOVE', 'preempted':'SM_MOVE' }, remapping={'target_pose':'ball_pose'} ) def execute(self, ud=None): if ud==None: ud = self.userdata self.action_client.wait_for_server() return Concurrence.execute(self, ud) def child_termination_cb(self, outmap): if outmap['CHASE_BALLS']!=: self.watcher.request_preempt() return True else: rospy.logwarn('Unexpected termination') return False def outcome_cb(self, outmap): return outmap['SM_MOVE']
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): self.heading = None """ Setting default values """ self.depth = None self.forward = None rospy.loginfo("Initializing mission planner interaction module...") rospy.Subscriber(name=header.ESTIMATED_POSE_TOPIC_NAME,data_class= krakenPose , callback=self.positionCallback, queue_size=100) self.setPointController=SimpleActionClient(header.CONTROLLER_SERVER, controllerAction) rospy.loginfo("waiting for setPoint controller action Server") self.setPointController.wait_for_server() rospy.loginfo("Got controller server ") self.advancedControllerClient=SimpleActionClient(header.ADVANCED_CONTROLLER_SERVER, 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=header.IP_CONTROLLER_PUBLISHING_TOPIC, data_class=ipControllererror, queue_size=100) self.moveAlongService=rospy.ServiceProxy(name=header.MOVE_ALONG_SERVICE_NAME, 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=header.RESET_POSE_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=header.PREMAP_LOCATION_SERVICE, service_class=getLocation) rospy.loginfo("waiting for premap location Service") self.moveAlongService.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 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
class Planner: zones = collections.deque([]) 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 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 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 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): 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 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 __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 __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, queue_size=10) # 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 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): StateMachine.__init__(self, input_keys=['target_pose'], outcomes=['succeeded', 'preempted', 'aborted'] ) self.sm_move = Concurrence( input_keys=['target_pose'], output_keys=['ball_pose'], outcomes=['succeeded', 'preempted', 'aborted'], default_outcome='aborted', child_termination_cb = self.child_termination_cb, outcome_cb = self.outcome_cb ) self.action_client = SimpleActionClient('move_base', MoveBaseAction) #self.action_client = PretendActionClient() self.move_state = Move(self.action_client, 0.2) self.chaser_state = BallChaser(self.action_client, 0.2) self.userdata.msg_in = None self.watcher = BallTracker(self.move_state, self.chaser_state) with self.sm_move: Concurrence.add('BALL_WATCHER', self.watcher) Concurrence.add('MOVE_TO_GOAL', self.move_state) with self: StateMachine.add('SM_MOVE', self.sm_move) StateMachine.add('CHASE_BALLS', self.move_state, transitions={ 'succeeded':'SM_MOVE', 'aborted':'SM_MOVE', 'preempted':'SM_MOVE' }, remapping={'target_pose':'ball_pose'} )
class ActionDispatcher(object): def __init__(self, name): rospy.loginfo("Starting %s" % name) self.client = None action_list = rospy.get_param("han_action_dispatcher")["han_actions"] self.default_action = rospy.get_param("~default_action") self.dyn_srv = DynServer(HanActionDispatcherConfig, self.dyn_callback) self.servers = {} for a in action_list.items(): name = a[0] self.servers[name] = SimpleActionServer( name, MoveBaseAction, execute_cb=(lambda x: lambda msg: self.execute_cb( msg, x[0], x[1]["action"]))(a), auto_start=False) self.servers[name].register_preempt_callback(self.preempt_cb) self.servers[name].start() rospy.loginfo("done") 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 preempt_cb(self): if self.client != None: self.client.cancel_all_goals() self.client = None def dyn_callback(self, config, level): self.use_default = config["use_default"] return config
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): 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)
class InteractiveMarkerGoal(object): 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 transformPose(self, target_frame, pose, time=None): transform = self.tfBuffer.lookup_transform( target_frame, pose.header.frame_id, pose.header.stamp if time is not None else rospy.Time(0), rospy.Duration(1.0)) new_pose = do_transform_pose(pose, transform) return new_pose def makeBox(self, msg): marker = Marker() marker.type = Marker.SPHERE marker.scale.x = msg.scale * 0.2 marker.scale.y = msg.scale * 0.2 marker.scale.z = msg.scale * 0.2 marker.color.r = 0.5 marker.color.g = 0.5 marker.color.b = 0.5 marker.color.a = 0.5 return marker def makeBoxControl(self, msg): control = InteractiveMarkerControl() control.always_visible = True control.markers.append(self.makeBox(msg)) msg.controls.append(control) return control def make6DofMarker(self, interaction_mode, root_link, tip_link): def normed_q(x, y, z, w): return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w]) int_marker = InteractiveMarker() p = PoseStamped() p.header.frame_id = tip_link p.pose.orientation.w = 1 int_marker.header.frame_id = tip_link int_marker.pose.orientation.w = 1 int_marker.pose = self.transformPose(root_link, p).pose int_marker.header.frame_id = root_link int_marker.scale = .2 int_marker.name = "eef_{}_to_{}".format(root_link, tip_link) # insert a box self.makeBoxControl(int_marker) int_marker.controls[0].interaction_mode = interaction_mode if interaction_mode != InteractiveMarkerControl.NONE: control_modes_dict = { InteractiveMarkerControl.MOVE_3D: "MOVE_3D", InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D", InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D" } int_marker.name += "_" + control_modes_dict[interaction_mode] control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "rotate_x" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(0, 0, 0, 1) control.name = "move_x" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = "rotate_z" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 1, 0, 1)) control.name = "move_z" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = "rotate_y" control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS int_marker.controls.append(control) control = InteractiveMarkerControl() control.orientation = Quaternion(*normed_q(0, 0, 1, 1)) control.name = "move_y" control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS int_marker.controls.append(control) return int_marker class process_feedback(object): def __init__(self, i_server, client, root_link, tip_link): self.i_server = i_server self.client = client self.tip_link = tip_link self.root_link = root_link def __call__(self, feedback): if feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: print('interactive goal update') goal = ControllerListGoal() goal.type = ControllerListGoal.STANDARD_CONTROLLER # asd = 'asd' # translation controller = Controller() controller.type = Controller.TRANSLATION_3D controller.tip_link = self.tip_link controller.root_link = self.root_link controller.goal_pose.header = feedback.header controller.goal_pose.pose = feedback.pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.05 controller.weight = 1.0 goal.controllers.append(controller) # rotation controller = Controller() controller.type = Controller.ROTATION_3D controller.tip_link = self.tip_link controller.root_link = self.root_link controller.goal_pose.header = feedback.header controller.goal_pose.pose = feedback.pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.2 controller.weight = 1.0 goal.controllers.append(controller) self.client.send_goal(goal) self.i_server.applyChanges()
class Test(object): 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 send_cart_goal(self, goal_pose): goal = MoveGoal() goal.type = MoveGoal.PLAN_AND_EXECUTE # translaiton controller = Controller() controller.type = Controller.TRANSLATION_3D controller.tip_link = 'gripper_tool_frame' controller.root_link = 'base_footprint' controller.goal_pose = goal_pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.05 goal.cmd_seq.append(MoveCmd()) goal.cmd_seq[-1].controllers.append(controller) # rotation controller = Controller() controller.type = Controller.ROTATION_3D controller.tip_link = 'gripper_tool_frame' controller.root_link = 'base_footprint' controller.goal_pose = goal_pose controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.2 goal.cmd_seq.append(MoveCmd()) goal.cmd_seq[-1].controllers.append(controller) self.client.send_goal(goal) result = self.client.wait_for_result(rospy.Duration(10)) print('finished in 10s?: {}'.format(result)) def send_rnd_joint_goal(self): goal = MoveGoal() goal.type = MoveGoal.PLAN_AND_EXECUTE # translation controller = Controller() controller.type = Controller.JOINT controller.tip_link = 'gripper_tool_frame' controller.root_link = 'base_footprint' for i, joint_name in enumerate(self.joint_names): controller.goal_state.name.append(joint_name) # controller.goal_state.position.append(0) controller.goal_state.position.append(np.random.random() - 0.5) controller.p_gain = 3 controller.enable_error_threshold = True controller.threshold_value = 0.05 controller.weight = 1 goal.cmd_seq.append(MoveCmd()) goal.cmd_seq[-1].controllers.append(controller) self.client.send_goal(goal) result = self.client.wait_for_result() final_js = rospy.wait_for_message( '/whole_body_controller/state', JointTrajectoryControllerState ) # type: JointTrajectoryControllerState asdf = {} for i, joint_name in enumerate(final_js.joint_names): asdf[joint_name] = final_js.actual.positions[i] for i, joint_name in enumerate(controller.goal_state.name): print('{} real:{} | exp:{}'.format( joint_name, asdf[joint_name], controller.goal_state.position[i])) print('finished in 10s?: {}'.format(result))
def __init__(self, action_server_name): # action server self.client = SimpleActionClient(action_server_name, ControllerListAction) self.client.wait_for_server()
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()
class Hexapod(object): """ Attributes hebi_group_name (str): hebi_mapping (list of list of str): leg_base_links (list of str): leg_end_links (list of str): """ 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 stand_up(self): rospy.loginfo("Hexapod standing up...") current_leg_positions = self._get_joint_angles() goal = TrajectoryGoal() start_wp = WaypointMsg() start_wp.names = self.hebi_mapping_flat start_wp.positions = self._flatten(current_leg_positions) start_wp.velocities = [0.0] * ACTUATORS_TOTAL start_wp.accelerations = [0.0] * ACTUATORS_TOTAL goal.waypoints.append(start_wp) goal.times.append(0.0) end_wp = WaypointMsg() end_wp.names = self.hebi_mapping_flat end_wp.positions = self._flatten(self.leg_jt_home_pos) end_wp.velocities = [0.0] * ACTUATORS_TOTAL end_wp.accelerations = [0.0] * ACTUATORS_TOTAL goal.waypoints.append(end_wp) goal.times.append(4.0) self.trajectory_action_client.send_goal( goal) # TODO: Add the various callbacks self.trajectory_action_client.wait_for_result() self.hold_pos([1, 2, 3, 4, 5, 6]) def loop(self): """Main Hexapod loop (distant - somewhat less accomplished - relative of HEBI algorithm) - Get chassis translation - Get leg end-effector translations (relative to leg base link) - side_alpha:odd, side_beta:even or side_alpha:even, side_beta:odd - side_alpha legs lift, plant to +transformation - side_alpha legs push to new home positions; side_beta legs push to -transformation - swap side_alpha and side_beta """ rospy.loginfo("Hexapod entering main loop...") rospy.loginfo( " Waiting for initial velocity command on /hexapod/cmd_vel/ ...") while self.last_vel_cmd is None: self._loop_rate.sleep() # start main loop while not rospy.is_shutdown(): chassis_pos_delta = None if self.last_vel_cmd is not None: dt = 1 # FIXME: Temporary for debugging lin_disp_lmt = self.linear_displacement_limit ang_disp_lmt = self.angular_displacement_limit chassis_pos_delta = Twist() chassis_pos_delta.linear.x = clamp( self.last_vel_cmd.linear.x * dt, -lin_disp_lmt, lin_disp_lmt) chassis_pos_delta.linear.y = clamp( self.last_vel_cmd.linear.y * dt, -lin_disp_lmt, lin_disp_lmt) chassis_pos_delta.linear.z = clamp( self.last_vel_cmd.linear.z * dt, -lin_disp_lmt, lin_disp_lmt) chassis_pos_delta.angular.x = clamp( self.last_vel_cmd.angular.x * dt, -ang_disp_lmt, ang_disp_lmt) chassis_pos_delta.angular.y = clamp( self.last_vel_cmd.angular.y * dt, -ang_disp_lmt, ang_disp_lmt) chassis_pos_delta.angular.z = clamp( self.last_vel_cmd.angular.z * dt, -ang_disp_lmt, ang_disp_lmt) self.last_vel_cmd = None if chassis_pos_delta is not None \ and not self._check_if_twist_msg_is_zero(chassis_pos_delta, linear_threshold=0.005, angular_threshold=0.01): # Get chassis position transformation chassis_pos_rot = transforms.euler_matrix( chassis_pos_delta.angular.x, chassis_pos_delta.angular.y, chassis_pos_delta.angular.z)[:3, :3] rospy.loginfo("chassis_pos_rot: %s", chassis_pos_rot) chassis_pos_trans = np.zeros([3]) chassis_pos_trans[0] = chassis_pos_delta.linear.x chassis_pos_trans[1] = chassis_pos_delta.linear.y chassis_pos_trans[2] = chassis_pos_delta.linear.z chassis_translation = np.dot(chassis_pos_trans, chassis_pos_rot) rospy.loginfo("chassis_translation: %s", chassis_translation) leg_target_eff_translation = [[]] * LEGS # Get leg base positions relative to chassis leg_base_positions = self._get_base_to_leg_base_fk() for i, leg_base_pos in enumerate(leg_base_positions): leg_base_pos_arr = np.array(leg_base_pos).reshape(3, 1) leg_base_pos_arr_new = np.dot(chassis_pos_rot, leg_base_pos_arr) leg_base_pos_trans_4 = np.ones(4).reshape(4, 1) leg_base_pos_trans_4[:3, :] = leg_base_pos_arr_new # get leg base translations relative to leg_base coordinate frame relative_trans = np.dot( np.linalg.inv(self.kdl_fk_base_to_leg_base[i].forward( [])), leg_base_pos_trans_4) relative_trans = relative_trans.reshape(1, 4).tolist()[0][:3] leg_target_eff_translation[i] = relative_trans # Get leg target end-effector translations for i, q in enumerate(self.leg_jt_home_pos): base_to_leg_base_rot = self.kdl_fk_base_to_leg_base[ i].forward([])[:3, :3] leg_target_eff_trans = np.dot( np.linalg.inv(base_to_leg_base_rot), chassis_translation).tolist()[0] leg_target_eff_translation[i] = [ x + y for x, y in zip(leg_target_eff_translation[i], leg_target_eff_trans) ] # TODO: FIXME: Technically incorrect # 1: side_alpha legs lift, plant to +transformation rospy.loginfo( "1: side_alpha legs lift, plant to +transformation") if self._odd_starts: active_legs = [1, 2, 5] else: # even starts active_legs = [0, 3, 4] init_wp = WaypointMsg() lift_wp = WaypointMsg() end_wp = WaypointMsg() legs_jt_init_pos = self._get_joint_angles() leg_eff_cur_pos = self._get_leg_base_to_eff_fk( legs_jt_init_pos) for i in range(LEGS): motor_names = [name for name in self.hebi_mapping[i]] # INITIAL POSITION init_wp.names.extend(motor_names) init_wp.positions.extend(legs_jt_init_pos[i]) init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) # LIFT lift_wp.names.extend(motor_names) if i in active_legs: # apply translation leg_lift_eff_target_pos = [ (x + y + z) / 2.0 for x, y, z in zip( leg_eff_cur_pos[i], self.leg_eff_home_pos[i], leg_target_eff_translation[i]) ] leg_lift_eff_target_pos = [ x + y for x, y in zip(leg_lift_eff_target_pos, self.leg_eff_step_height[i]) ] # get ik leg_lift_jt_target_pos = self._get_pos_ik( self.trac_ik_leg_base_to_end[i], legs_jt_init_pos[i], leg_lift_eff_target_pos, seed_xyz=self.leg_eff_home_pos[i]) lift_wp.positions.extend(leg_lift_jt_target_pos) lift_wp.velocities.extend([NAN] * ACTUATORS_PER_LEG) lift_wp.accelerations.extend([NAN] * ACTUATORS_PER_LEG) else: lift_wp.positions.extend(legs_jt_init_pos[i]) lift_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) lift_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) # PLANT end_wp.names.extend(motor_names) if i in active_legs: # apply translation leg_plant_eff_target_pos = [ x + y for x, y in zip(self.leg_eff_home_pos[i], leg_target_eff_translation[i]) ] leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][ 2] # end eff z-position should match home z-position # get ik leg_plant_jt_target_pos = self._get_pos_ik( self.trac_ik_leg_base_to_end[i], leg_lift_jt_target_pos, leg_plant_eff_target_pos, seed_xyz=leg_lift_eff_target_pos) end_wp.positions.extend(leg_plant_jt_target_pos) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) else: end_wp.positions.extend(legs_jt_init_pos[i]) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) goal = TrajectoryGoal() goal.waypoints.append(init_wp) goal.waypoints.append(lift_wp) goal.waypoints.append(end_wp) goal.times.extend([0.0, 0.4, 0.8]) self.release_pos([1, 2, 3, 4, 5, 6]) self.trajectory_action_client.send_goal(goal) self.trajectory_action_client.wait_for_result() self.hold_pos([1, 2, 3, 4, 5, 6]) # 2: side_alpha legs push to new home positions; side_beta legs push to -transformation rospy.loginfo( "2: side_alpha legs push to new home positions; side_beta legs push to -transformation" ) if self._odd_starts: active_legs = [0, 3, 4] else: # even starts active_legs = [1, 2, 5] init_wp = WaypointMsg() end_wp = WaypointMsg() legs_jt_init_pos = self._get_joint_angles() for i in range(LEGS): motor_names = [name for name in self.hebi_mapping[i]] # INITIAL POSITION init_wp.names.extend(motor_names) init_wp.positions.extend(legs_jt_init_pos[i]) init_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) init_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) # PUSH end_wp.names.extend(motor_names) if i in active_legs: # apply -translation leg_plant_eff_target_pos = [ x + y for x, y in zip(self.leg_eff_home_pos[i], [ -val for val in leg_target_eff_translation[i] ]) ] leg_plant_eff_target_pos[2] = self.leg_eff_home_pos[i][ 2] # end eff z-position should match home z-position # get ik leg_plant_jt_target_pos = self._get_pos_ik( self.trac_ik_leg_base_to_end[i], legs_jt_init_pos[i], leg_plant_eff_target_pos, seed_xyz=self.leg_eff_home_pos[i]) end_wp.positions.extend(leg_plant_jt_target_pos) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) else: end_wp.positions.extend(self.leg_jt_home_pos[i]) end_wp.velocities.extend([0.0] * ACTUATORS_PER_LEG) end_wp.accelerations.extend([0.0] * ACTUATORS_PER_LEG) goal = TrajectoryGoal() goal.waypoints.append(init_wp) goal.waypoints.append(end_wp) goal.times.extend([0.0, 0.4]) self.release_pos([1, 2, 3, 4, 5, 6]) self.trajectory_action_client.send_goal(goal) self.trajectory_action_client.wait_for_result() self.hold_pos([1, 2, 3, 4, 5, 6]) self._odd_starts = not self._odd_starts self._loop_rate.sleep( ) # FIXME: Doesn't make sense to use this unless re-planning trajectories # end main loop def _get_pos_ik(self, ik_solver, seed_angles, target_xyz, target_wxyz=None, seed_xyz=None, recursion_depth_cnt=100): if recursion_depth_cnt < 0: rospy.logdebug("%s FAILURE. Maximum recursion depth reached", self._get_pos_ik.__name__) return None rospy.logdebug("recursion depth = %s", recursion_depth_cnt) if target_wxyz is None: target_wxyz = [ 1, 0, 0, 0 ] # trak-ik seems a little more stable when given initial pose for pos-only ik target_jt_angles = ik_solver.get_ik( seed_angles, target_xyz[0], target_xyz[1], target_xyz[2], target_wxyz[0], target_wxyz[1], target_wxyz[2], target_wxyz[3], self.ik_pos_xyz_bounds[0], self.ik_pos_xyz_bounds[1], self.ik_pos_xyz_bounds[2], self.ik_pos_wxyz_bounds[0], self.ik_pos_wxyz_bounds[1], self.ik_pos_wxyz_bounds[2]) if target_jt_angles is not None: # ik_solver succeeded rospy.logdebug( "%s SUCCESS. Solution: %s to target xyz: %s from seed angles: %s", self._get_pos_ik.__name__, round_list(target_jt_angles, 4), round_list(target_xyz, 4), round_list(seed_angles, 4)) return target_jt_angles else: # ik_solver failed if seed_xyz is None: rospy.logdebug( "%s FAILURE. Solution: %s to target_xyz: %s from seed_angles: %s", self._get_pos_ik.__name__, ['NA', 'NA', 'NA'], target_xyz, seed_angles) return target_jt_angles else: # binary recursive search target_xyz_new = [(x + y) / 2.0 for x, y in zip(target_xyz, seed_xyz)] recursive_jt_angles = self._get_pos_ik(ik_solver, seed_angles, target_xyz_new, target_wxyz, seed_xyz, recursion_depth_cnt - 1) if recursive_jt_angles is None: rospy.logdebug( "%s FAILURE. Solution: %s to target_xyz: %s from seed_angles: %s", self._get_pos_ik.__name__, ['NA', 'NA', 'NA'], round_list(target_xyz, 4), round_list(seed_angles, 4)) else: return self._get_pos_ik(ik_solver, recursive_jt_angles, target_xyz, target_wxyz, target_xyz_new, recursion_depth_cnt - 1) def _get_base_to_leg_base_fk(self): leg_base_pos = [[]] * LEGS for i, fk_solver in enumerate(self.kdl_fk_base_to_leg_base): base_to_leg_base_tf = fk_solver.forward([]) leg_base_pos[i] = base_to_leg_base_tf[:3, 3].reshape(1, 3).tolist()[0] return leg_base_pos def _get_leg_base_to_eff_fk(self, jt_angles): leg_eff_cur_pos = [[]] * LEGS # relative to leg base for i, (fk_solver, angles) \ in enumerate(zip(self.kdl_fk_leg_base_to_eff, jt_angles)): leg_base_to_eff_tf = fk_solver.forward(angles) leg_eff_cur_pos[i] = leg_base_to_eff_tf[:3, 3].reshape(1, 3).tolist()[0] return leg_eff_cur_pos def _get_base_to_leg_eff_fk(self, jt_angles): leg_eff_cur_pos = [[]] * LEGS # relative to base for i, (fk_solver_1, fk_solver_2, angles) \ in enumerate(zip(self.kdl_fk_base_to_leg_base, self.kdl_fk_leg_base_to_eff, jt_angles)): base_to_leg_base_tf = fk_solver_1.forward([]) leg_base_to_eff_tf = fk_solver_2.forward(angles) base_to_eff_tf = np.dot(base_to_leg_base_tf, leg_base_to_eff_tf) leg_eff_cur_pos[i] = base_to_eff_tf[:3, 3].reshape(1, 3).tolist()[0] return leg_eff_cur_pos def hold_pos(self, leg_nums): released_leg = False for num in leg_nums: num -= 1 # Convert from 1-based leg numbering to 0-based indexing if not self._hold_leg_list[num]: released_leg = True break if released_leg: self._hold_leg_positions = self._get_joint_angles() for num in leg_nums: num -= 1 # Convert from 1-based leg numbering to 0-based indexing self._hold_leg_list[num] = True def release_pos(self, leg_nums): for num in leg_nums: num -= 1 # Convert from 1-based leg numbering to 0-based indexing self._hold_leg_list[num] = False def _get_joint_angles(self): return [[self._current_jt_pos[motor] for motor in leg] for leg in self.hebi_mapping] def _get_joint_velocities(self): return [[self._current_jt_vel[motor] for motor in leg] for leg in self.hebi_mapping] def _get_joint_efforts(self): return [[self._current_jt_eff[motor] for motor in leg] for leg in self.hebi_mapping] @staticmethod def _get_list_of_lists(item=None): return [[item] * ACTUATORS_PER_LEG] * LEGS @staticmethod def _flatten(listoflists): return [item for lst in listoflists for item in lst] def _feedback_cb(self, msg): for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity, msg.effort): if name not in self.hebi_mapping_flat: print("WARNING: arm_callback - unrecognized name!!!") else: self._current_jt_pos[name] = pos self._current_jt_vel[name] = vel self._current_jt_eff[name] = eff # Publish JointState() for RViz if not rospy.is_shutdown( ) and self._joint_state_pub is not None: jointstate = JointState() jointstate.header.stamp = rospy.Time.now() jointstate.name = self.hebi_mapping_flat jointstate.position = self._flatten( self._get_joint_angles()) jointstate.velocity = [0.0] * len(jointstate.name) jointstate.effort = [0.0] * len(jointstate.name) self._joint_state_pub.publish(jointstate) # Publish JointState() for held legs # TODO: Probably better to put in its own callback # TODO: Trigger at regular intervals and when self._hold_leg_list changes # TODO: Smooth transition from trajectory to cmd if not rospy.is_shutdown( ) and self._joint_state_pub is not None and any(self._hold_leg_list): jointstate = JointState() jointstate.header.stamp = rospy.Time.now() for i, leg in enumerate(self.hebi_mapping): if self._hold_leg_list[i]: jointstate.name.extend(leg) jointstate.position.extend(self._hold_leg_positions[i]) jointstate.velocity = [] jointstate.effort = [] # TODO: Gravity compensation? self.cmd_pub.publish(jointstate) def _cmd_vel_cb(self, msg): if isinstance(msg, Twist): if self.last_vel_cmd is None: self.last_vel_cmd = Twist() self.last_vel_cmd.linear.x = msg.linear.x self.last_vel_cmd.linear.y = msg.linear.y self.last_vel_cmd.linear.z = msg.linear.z self.last_vel_cmd.angular.x = msg.angular.x self.last_vel_cmd.angular.y = msg.angular.y self.last_vel_cmd.angular.z = msg.angular.z @staticmethod def _check_if_twist_msg_is_zero(twist_msg, linear_threshold, angular_threshold): assert isinstance(twist_msg, Twist) if abs(twist_msg.linear.x) > linear_threshold: return False elif abs(twist_msg.linear.y) > linear_threshold: return False elif abs(twist_msg.linear.z) > linear_threshold: return False elif abs(twist_msg.angular.x) > angular_threshold: return False elif abs(twist_msg.angular.y) > angular_threshold: return False elif abs(twist_msg.angular.z) > angular_threshold: return False else: return True
class TrajectoryRecorder(object): """ Attributes hebi_group_name (str): hebi_families (list of str): HEBI actuator families [base,..., tip] hebi_names (list of str): HEBI actuator names [base,..., tip] """ 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 set_waypoint(self): waypoint = Waypoint() joint_state = waypoint.joint_state joint_state.names = self.hebi_mapping joint_state.positions = self._get_joint_angles() if self.robot_urdf is not None: homogeneous_transform = self.kdl_kin.forward(joint_state.positions) waypoint.end_effector_pose = self._get_pose_from_homogeneous_matrix( homogeneous_transform) print("Store these joint positions as Waypoint #{}?:".format( self.waypoints_cnt + 1)) for name, pos in zip(self.hebi_mapping, joint_state.positions): print(" {:20}: {:4f}".format(name, pos)) user_input = self.raw_input_ros_safe( "[Return] - yes | [Any other key] - no\n") if user_input != "" and user_input != " ": self.release_position() return False self.hold_position() valid_input = False print( "\nPlease enter velocities (optional) for Waypoint #{} in the following format:" .format(self.waypoints_cnt + 1)) print("Ex: 0,none,0") user_input = self.raw_input_ros_safe( "[Return] - skip | velocity1,velocity2, ...velocityN\n") while not rospy.is_shutdown() and not valid_input: if user_input == "" or user_input == " ": joint_state.velocities = [NAN] * len(self.hebi_mapping) valid_input = True elif len(user_input.split(",")) != len(self.hebi_mapping): print( " INVALID INPUT: velocity list must be the same size as module list" ) print(" Please try again.") user_input = self.raw_input_ros_safe( "[Return] - skip | velocity1,velocity2, ...velocityN\n") else: joint_state.velocities = [ NAN if vel.strip().lower() == 'none' else float(vel) for vel in user_input.split(",") ] valid_input = True valid_input = False print( "\nPlease enter accelerations (optional) for Waypoint #{} in the following format:" .format(self.waypoints_cnt + 1)) print("Ex: 0,1.1,none") user_input = self.raw_input_ros_safe( "[Return] - skip | accelerations1,accelerations2, ...accelerationsN\n" ) while not rospy.is_shutdown() and not valid_input: if user_input == "" or user_input == " ": joint_state.accelerations = [NAN] * len(self.hebi_mapping) valid_input = True elif len(user_input.split(",")) != len(self.hebi_mapping): print( " INVALID INPUT: acceleration list must be the same size as module list" ) print(" Please try again.") user_input = self.raw_input_ros_safe( "[Return] - skip | accelerations1,accelerations2, ... accelerationsN\n" ) else: joint_state.accelerations = [ NAN if acc.strip().lower() == "none" else float(acc) for acc in user_input.split(",") ] valid_input = True user_input = self.raw_input_ros_safe( "\nPlease enter a duration [s] - time to move from the previous waypoint to the current waypoint:\n" ) while not rospy.is_shutdown() and (user_input == "" or user_input == " "): user_input = self.raw_input_ros_safe( "Please enter a duration [s] - time to move from the previous waypoint to the current waypoint:\n" ) waypoint.time = float(user_input) self.append_waypoint(waypoint) print("\nWaypoint #{} stored!\n".format(self.waypoints_cnt)) self.release_position() return True @staticmethod def raw_input_ros_safe(prompt=None): sys.stdout.flush() try: if prompt is not None: user_input = raw_input(prompt) else: user_input = raw_input() sys.stdout.flush() return user_input except KeyboardInterrupt: sys.exit() def append_waypoint(self, waypoint): self.waypoints.append(waypoint) self.waypoints_cnt += 1 self.int_markers_man.add_waypoint_marker(waypoint, str(self.waypoints_cnt)) self._prev_breakpoint = False def append_breakpoint(self): if self._prev_breakpoint: print("Error: Cannot set two consecutive breakpoints!") else: self._prev_breakpoint = True self.waypoints.append(None) self.breakpoints_cnt += 1 print("\nBreakpoint #{} stored!\n".format(self.breakpoints_cnt)) def record_movement(self): resolution_xyz = None if self.robot_urdf is not None: resolution_xyz = 0.01 * float( self.raw_input_ros_safe( "Please enter the desired end-effector Cartesian resolution [cm]:\n" )) # TODO: Add some user-input checking functions resolution_jt = (m.pi / 180) * float( self.raw_input_ros_safe( "Please enter the desired joint resolution [degrees]:\n")) duration = float( self.raw_input_ros_safe( "Please enter a setup duration for this movement [s]:\n")) # Set up Thread user_input = [None] t = Thread(target=self._wait_for_user_input, args=(user_input, )) t.start() # first post prev_jt_angles = self._get_joint_angles() prev_end_effector_xyz = None waypoint = Waypoint() joint_state = waypoint.joint_state joint_state.names = self.hebi_mapping joint_state.positions = prev_jt_angles joint_state.velocities = self._get_joint_velocities() joint_state.accelerations = [NAN] * len( self.hebi_mapping) # TODO: Can we get these from anywhere? if self.robot_urdf is not None: homogeneous_transform = self.kdl_kin.forward(joint_state.positions) waypoint.end_effector_pose = self._get_pose_from_homogeneous_matrix( homogeneous_transform) eff_pos = waypoint.end_effector_pose.position prev_end_effector_xyz = [eff_pos.x, eff_pos.y, eff_pos.z] waypoint.time = duration self.append_waypoint(waypoint) prev_time = rospy.Time.now().to_sec() # subsequent posts while not rospy.is_shutdown() and user_input[0] is None: jt_angles = self._get_joint_angles() end_effector_pose = None end_effector_xyz = None xyz_dist = None if self.robot_urdf is not None: homogeneous_transform = self.kdl_kin.forward( joint_state.positions) end_effector_pose = self._get_pose_from_homogeneous_matrix( homogeneous_transform) eff_pos = waypoint.end_effector_pose.position end_effector_xyz = [eff_pos.x, eff_pos.y, eff_pos.z] xyz_dist = self._get_abs_distance(end_effector_xyz, prev_end_effector_xyz) jt_dist = self._get_abs_distance(jt_angles, prev_jt_angles) if (self.robot_urdf is not None and xyz_dist > resolution_xyz) or jt_dist > resolution_jt: cur_time = rospy.Time.now().to_sec() waypoint = Waypoint() joint_state = waypoint.joint_state joint_state.positions = jt_angles joint_state.velocities = self._get_joint_velocities() joint_state.accelerations = [NAN] * len( self.hebi_mapping) # TODO: Can we get these from anywhere? waypoint.end_effector_pose = end_effector_pose waypoint.time = cur_time - prev_time self.append_waypoint(waypoint) prev_jt_angles = jt_angles prev_end_effector_xyz = end_effector_xyz prev_time = cur_time t.join() @staticmethod def _get_abs_distance(vector1, vector2): assert len(vector1) == len(vector2) sum_of_squares = 0.0 for i, (val1, val2) in enumerate(zip(vector1, vector2)): sum_of_squares += (val1 - val2)**2 return m.sqrt(sum_of_squares) @staticmethod def _wait_for_user_input(user_input): user_input[0] = raw_input("Press [Return] to stop recording!!!\n") @staticmethod def _get_pose_from_homogeneous_matrix(homogeneous_transform_matrix): pose = Pose() pose.position.x = round(homogeneous_transform_matrix[0, 3], 6) pose.position.y = round(homogeneous_transform_matrix[1, 3], 6) pose.position.z = round(homogeneous_transform_matrix[2, 3], 6) quaternion = transforms.quaternion_from_matrix( homogeneous_transform_matrix[:3, :3]) # TODO: Check me pose.orientation.w = quaternion[0] pose.orientation.x = quaternion[1] pose.orientation.y = quaternion[2] pose.orientation.z = quaternion[3] return pose def print_waypoints(self): breakpoints_passed = 0 time_from_start = 0.0 for i, waypoint in enumerate(self.waypoints): if waypoint is not None: time_from_start += waypoint.time print("\nWaypoint #{} at time {} [s] from trajectory start". format(i + 1 - breakpoints_passed, time_from_start)) joint_state = waypoint.joint_state table = [[name, pos, vel, acc] for name, pos, vel, acc in zip( joint_state.names, joint_state.positions, joint_state.velocities, joint_state.accelerations)] print( tabulate(table, headers=[ 'Names', 'Positions [rad]', 'Velocities [rad/s]', 'Accelerations [rad/s^2]' ])) eff_position = waypoint.end_effector_pose.position print("\nEnd effector position x={}, y={}, z={}".format( eff_position.x, eff_position.y, eff_position.z)) print( "-" * (len("\nWaypoint #{} at time {} [s] from trajectory start") - 4 + len(str(i) + str(time_from_start)))) else: breakpoints_passed += 1 print("\nBreakpoint #{}".format(breakpoints_passed)) print(("-" * (len("\nBreakpoint #{}") - 4 + len(str(i))))) def execute_trajectories(self): trajectory_goals, tmp = self._get_trajectory_and_end_effector_goals_from_waypoints( ) # execute initial position-to-start trajectory if len(trajectory_goals) == 0: print("Error: No trajectory goals to execute!") else: # execute initial position to start trajectory init_goal = TrajectoryGoal() waypoint_1 = WaypointMsg() waypoint_1.names = self.hebi_mapping waypoint_1.positions = self._get_joint_angles() waypoint_1.velocities = [0] * len(self.hebi_mapping) waypoint_1.accelerations = [0] * len(self.hebi_mapping) waypoint_2 = trajectory_goals[0].waypoints[0] init_goal.waypoints = [waypoint_1, waypoint_2] init_goal.times = [0, 3] self._executing_trajectory = True self.trajectory_action_client.send_goal(init_goal) self.trajectory_action_client.wait_for_result() # execute trajectory goals for goal in trajectory_goals: self.trajectory_action_client.send_goal(goal) self.trajectory_action_client.wait_for_result() self.trajectory_action_client.get_result() self._executing_trajectory = False def execute_trajectory(self): # TODO: Maybe support executing individual trajectories. Project creep... this is just a command line tool... pass def save_trajectory_to_file(self): # Get target package path rospack = RosPack() target_package_found = False package_path = None while not target_package_found: target_package_name = raw_input("Please enter target package: ") try: package_path = rospack.get_path(target_package_name) target_package_found = True print("Target package path: {}".format(package_path)) except ResourceNotFound: print("Package {} not found!!! Please try again.".format( target_package_name)) # Create trajectories directory if it does not exist - https://stackoverflow.com/a/14364249 save_dir_path = os.path.join(package_path, "trajectories") print("Save directory path: {}".format(save_dir_path)) try: # prevents a common race condition - duplicated attempt to create directory os.makedirs(save_dir_path) except OSError: if not os.path.isdir(save_dir_path): raise # Get save file name name_set = False base_filename = None while not name_set: base_filename = raw_input("Please enter the save file name: ") target_name_path_1 = os.path.join(save_dir_path, base_filename + ".json") target_name_path_2 = os.path.join(save_dir_path, base_filename + "_0.json") if os.path.isfile(target_name_path_1) or os.path.isfile( target_name_path_2): print( "A file with name {} already exists!!! Please try again.". format(base_filename)) else: name_set = True # Dump trajectory artifacts to file(s) trajectory_goals, end_effector_goals = self._get_trajectory_and_end_effector_goals_from_waypoints( ) for i, (trajectory_goal, end_effector_goal) in enumerate( zip(trajectory_goals, end_effector_goals)): suffix = "_" + str(i) path = os.path.join(save_dir_path, base_filename + suffix + ".json") print("Saving trajectory {} to: {}\n".format(i, path)) with open(path, 'w') as savefile: json_data_structure = self._convert_trajectory_goal_to_json_serializable( trajectory_goal, end_effector_goal) json_str = json.dumps(json_data_structure, sort_keys=True, indent=4, separators=(',', ': ')) match_re = r'(\n*\s*\[)(\n\s*("*[\w\./\-\d]+"*,*\n\s*)+\])' reformatted_json_str = re.sub(match_re, self._newlinereplace, json_str) savefile.write(reformatted_json_str) def _convert_trajectory_goal_to_json_serializable(self, trajectory_goal, end_effector_goal): waypoints_dict = {} for i, (waypoint, pose) in enumerate( zip(trajectory_goal.waypoints, end_effector_goal)): waypoint_dict = { 'names': list(waypoint.names), 'positions': [float(positions) for positions in waypoint.positions], 'velocities': [float(velocities) for velocities in waypoint.velocities], 'accelerations': [ float(acceleration) for acceleration in waypoint.accelerations ] } if self.robot_urdf is not None: eff_position = pose.position waypoint_dict['end-effector xyz'] = [ eff_position.x, eff_position.y, eff_position.z ] eff_orientation = pose.orientation waypoint_dict['end-effector wxyz'] = [ eff_orientation.w, eff_orientation.x, eff_orientation.y, eff_orientation.z ] waypoints_dict[str(i)] = waypoint_dict times_list = [float(time) for time in trajectory_goal.times] json_data_structure = { 'waypoints': waypoints_dict, 'times': times_list } if self.chain_base_link_abs_transform is not None: json_data_structure[ 'base link transform'] = self.chain_base_link_abs_transform.tolist( ) return json_data_structure @staticmethod def _newlinereplace(matchobj): no_newlines = matchobj.group(2).replace( "\n", "") # eliminate newline characters no_newlines = no_newlines.split() # eliminate excess whitespace no_newlines = "".join([" " + segment for segment in no_newlines]) return matchobj.group(1) + no_newlines def _get_trajectory_and_end_effector_goals_from_waypoints(self): trajectory_goals = [] end_effector_goals = [] prev_breakpoint = False prev_time = 0.0 for waypoint in self.waypoints: if waypoint is not None: if len(trajectory_goals) == 0 or prev_breakpoint: trajectory_goals.append(TrajectoryGoal()) end_effector_goals.append([]) # if applicable, 1st append last waypoint from previous trajectory if len(trajectory_goals) > 1 and prev_breakpoint: waypoint_msg = WaypointMsg() waypoint_msg.names = list( trajectory_goals[-2].waypoints[-1].names) waypoint_msg.velocities = [0] * len(waypoint_msg.names) waypoint_msg.accelerations = [0] * len(waypoint_msg.names) trajectory_goals[-1].waypoints.append(waypoint_msg) trajectory_goals[-1].times.append(0.0) end_effector_goals[-1].append( copy.deepcopy(end_effector_goals[-2])) # append a new waypoint trajectory_goals[-1].waypoints.append( copy.deepcopy(waypoint.joint_state)) prev_time += waypoint.time trajectory_goals[-1].times.append(prev_time) end_effector_goals[-1].append( copy.deepcopy(waypoint.end_effector_pose)) prev_breakpoint = False else: # breakpoint prev_time = 0.0 prev_breakpoint = True return trajectory_goals, end_effector_goals def delete_waypoint(self, waypoint): index = self.waypoints.index(waypoint) # Get index of last waypoint deleting_last_wp = False for i, wp in reversed(list(enumerate(self.waypoints))): if wp is not None: if i == index: deleting_last_wp = True break self.waypoints.remove(waypoint) self.waypoints_cnt -= 1 last_wp = None # Check for and remove adjacent breakpoints. Also record last waypoint prev_wp = None for i, wp in enumerate(list(self.waypoints)): if wp is None and prev_wp is None: del self.waypoints[i] self.breakpoints_cnt -= 1 else: last_wp = wp prev_wp = wp # Check if last waypoint is a breakpoint if len(self.waypoints) == 0 or self.waypoints[-1] is None: self._prev_breakpoint = True print("Waypoint deleted...") if len(self.waypoints) != 0 and index == 0: self.waypoints[0].joint_state.velocities = [0] * len( self.hebi_mapping) self.waypoints[0].joint_state.accelerations = [0] * len( self.hebi_mapping) self.waypoints[0].time = 0 # TODO: Fix time. Have to access waypoint marker menus, etc... elif deleting_last_wp and last_wp is not None: i = self.waypoints.index(last_wp) self.waypoints[i].joint_state.velocities = [0] * len( self.hebi_mapping) self.waypoints[i].joint_state.accelerations = [0] * len( self.hebi_mapping) def insert_waypoint(self, ref_waypoint, before=True): # create new Waypoint waypoint = Waypoint() waypoint.joint_state.names = self.hebi_mapping waypoint.joint_state.positions = self._get_joint_angles() waypoint.joint_state.velocities = [NAN] * len(self.hebi_mapping) waypoint.joint_state.accelerations = [NAN] * len(self.hebi_mapping) if self.robot_urdf is not None: homogeneous_transform = self.kdl_kin.forward( waypoint.joint_state.positions) waypoint.end_effector_pose = self._get_pose_from_homogeneous_matrix( homogeneous_transform) waypoint.time = 2.0 # TODO: Figure out the best way to edit this from GUI # determine index of reference waypoint ref_index = None wp_passed_cnt = 0 for i, wp in enumerate(self.waypoints): if wp is ref_waypoint: ref_index = i break elif wp is not None: wp_passed_cnt += 1 # insert new Waypoint if before: if ref_index == 0: # undo previous initial waypoint settings self.waypoints[0].joint_state.velocities = [NAN] * len( self.hebi_mapping) self.waypoints[0].joint_state.accelerations = [NAN] * len( self.hebi_mapping) self.waypoints[0].time = 2.0 # TODO: Fix time. Have to access waypoint marker menus, etc... # apply initial waypoint settings waypoint.joint_state.velocities = [0] * len(self.hebi_mapping) waypoint.joint_state.accelerations = [0] * len( self.hebi_mapping) waypoint.time = 0 self.waypoints.insert(ref_index, waypoint) else: if ref_index < len(self.waypoints) - 1: self.waypoints.insert(ref_index + 1, waypoint) else: # undo previous initial waypoint settings self.waypoints[-1].joint_state.velocities = [NAN] * len( self.hebi_mapping) self.waypoints[-1].joint_state.accelerations = [NAN] * len( self.hebi_mapping) # apply final waypoint settings waypoint.joint_state.velocities = [0] * len(self.hebi_mapping) waypoint.joint_state.accelerations = [0] * len( self.hebi_mapping) self.waypoints.append(waypoint) wp_passed_cnt += 1 self.waypoints_cnt += 1 self.int_markers_man.add_waypoint_marker( waypoint, description=str(wp_passed_cnt + 1)) self._prev_breakpoint = False def restart(self): self.waypoints = [] self.waypoints_cnt = 0 self.breakpoints_cnt = 0 self._prev_breakpoint = True self.release_position() self.int_markers_man.clear_waypoint_markers() def hold_position(self): self._hold_joint_states = self._get_joint_angles() self._hold_position = True def release_position(self): self._hold_position = False def exit(self): self.release_position() def _get_joint_angles(self): return [self.current_jt_pos[motor] for motor in self.hebi_mapping] def _get_joint_velocities(self): return [self.current_jt_vel[motor] for motor in self.hebi_mapping] def _get_joint_efforts(self): return [self.current_jt_eff[motor] for motor in self.hebi_mapping] def _feedback_cb(self, msg): for name, pos, vel, eff in zip(msg.name, msg.position, msg.velocity, msg.effort): if name not in self.hebi_mapping: print("WARNING: arm_callback - unrecognized name!!!") else: self.current_jt_pos[name] = pos self.current_jt_vel[name] = vel self.current_jt_eff[name] = eff if not rospy.is_shutdown( ) and self._joint_state_pub is not None: # Publish JointState() for RViz jointstate = JointState() jointstate.header.stamp = rospy.Time.now() jointstate.name = self._active_joints jointstate.position = self._get_joint_angles() jointstate.velocity = [0.0] * len(jointstate.name) jointstate.effort = [0.0] * len(jointstate.name) self._joint_state_pub.publish(jointstate) # TODO: Make this less hacky if not rospy.is_shutdown() and self._joint_state_pub is not None: if not self._executing_trajectory: joint_grav_torques = self.kdl_kin.get_joint_torques_from_gravity( self._get_joint_angles(), grav=[0, 0, -9.81]) # TODO: FIXME: Hardcoded jointstate = JointState() jointstate.header.stamp = rospy.Time.now() jointstate.name = self.hebi_mapping if self._hold_position: # jointstate.position = self.waypoints[-1].positions # jerky jointstate.position = self._hold_joint_states else: jointstate.position = [] jointstate.velocity = [] jointstate.effort = joint_grav_torques self.cmd_pub.publish(jointstate)
class NavStall(): 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, queue_size=10) # 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 shutdown(self): self.cancelAndStop() ''' Implement leaky bucket for stall detection ''' def detect_stall(self, msg): now = rospy.Time.now() if msg.header.stamp.secs < (now.secs-1): #skip messages that are older than 1 sec (stale) return ''' if (self.hasGivenUp): self.givenUp() return #if we've been stalled for too long time, just give up if (self.stallCounterBucket > self.STOP_MAX_STALL_BUCKET_COUNT): self.hasGivenUp = 1 self.givenUp() return ''' if (msg.value > self.stall_current): self.stallCounterBucket+=2; rospy.loginfo("Potential stall condition detected at current: " + str(msg.value) + " (stall current: " + str(self.stall_current) + ") - incremented Stall Counter to " + str(self.stallCounterBucket)) else: if (self.stallCounterBucket > 0): self.stallCounterBucket-=1; #decrement bucket rospy.loginfo("Decremented Stall Counter to " + str(self.stallCounterBucket)) if (self.stallCounterBucket > self.MAX_STALL_BUCKET_COUNT): rospy.logwarn("Stall conditition detected! Trying to recover...") self.cancelAndStop() self.recover() rospy.logwarn("Stall recovery completed.") def recover(self): rospy.loginfo("Initiating recovery... Speed: " + str(self.recovery_speed) + " Time: " + str(self.recovery_time)) cmd_vel = Twist() #move back for one second at low speed cmd_vel.linear.x = -self.recovery_speed #need to repeat this multiple times, as base_controller will timeout after 0.5 sec for x in range(0, self.recovery_time*4): self.cmd_vel_pub.publish(cmd_vel) rospy.sleep(0.25) #stop rospy.loginfo("Stopping the robot after recovery move...") self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) def cancelAndStop(self): rospy.loginfo("Canceling all goals...") self.move_base.cancel_all_goals() rospy.sleep(1) rospy.loginfo("Stopping the robot...") self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) '''
class SimpleActionState(State): """Simple action client state. Use this class to represent an actionlib as a state in a state machine. """ # Meta-states for this action WAITING_FOR_SERVER = 0 INACTIVE = 1 ACTIVE = 2 PREEMPTING = 3 COMPLETED = 4 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, preempt_timeout = rospy.Duration(60.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 tobe 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 preempt_timeout: C{rospy.Duration} @param preempt_timeout: This is the timeout used for aborting after a preempt has been sent to the action and no result has been received. This timeout begins counting after a preempt message 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._exec_timeout = exec_timeout self._preempt_timeout = preempt_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") if not all([s in action_spec().action_goal.goal.__slots__ for s in goal_slots]): raise smach.InvalidStateError("Goal slots specified are not valid 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._preempt_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 # Condition variables for threading synchronization self._done_cond = threading.Condition() def _wait_for_server(self): """Internal method for waiting for the action server This is run in a separate thread and allows construction of this state to not block the construction of other states. """ while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown(): try: if self._action_client.wait_for_server(rospy.Duration(1.0)):#self._server_wait_timeout): self._status = SimpleActionState.INACTIVE if self.preempt_requested(): return except: if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called rospy.logerr("Failed to wait for action server '%s'" % (self._action_name)) def _execution_timer(self): """Internal method for cancelling a timed out goal after a timeout.""" while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown(): try: rospy.sleep(0.1) except: if not rospy.is_shutdown(): rospy.logerr("Failed to sleep while running '%s'" % self._action_name) if rospy.Time.now() - self._activate_time > self._exec_timeout: rospy.logwarn("Action %s timed out after %d seconds." % (self._action_name, self._exec_timeout.to_sec())) # Cancel the goal self._action_client.cancel_goal() ### smach State API def request_preempt(self): rospy.loginfo("Preempt requested on action '%s'" % (self._action_name)) smach.State.request_preempt(self) if self._status == SimpleActionState.ACTIVE: rospy.loginfo("Preempt on action '%s' cancelling goal: \n%s" % (self._action_name, str(self._goal))) # Cancel the goal self._action_client.cancel_goal() def execute(self, ud): """Called when executing a state. This calls the goal_cb if it is defined, and then dispatches the goal with a non-blocking call to the action client. """ # Make sure we're connected to the action server if self._status is SimpleActionState.WAITING_FOR_SERVER: rospy.logwarn("Still waiting for action server '%s' to start... is it running?" % self._action_name) if self._action_wait_thread.is_alive(): # Block on joining the server wait thread (This can be preempted) self._action_wait_thread.join() else: # Wait for the server in this thread (This can also be preempted) self._wait_for_server() rospy.loginfo("Connected to action server '%s'." % self._action_name) # Check for preemption before executing if self.preempt_requested(): rospy.loginfo("Preempting %s before sending goal." % self._action_name) self.service_preempt() return 'preempted' # We should only get here if we have connected to the server if self._status is SimpleActionState.WAITING_FOR_SERVER: rospy.logfatal("Action server for "+self._action_name+" is not running.") return 'aborted' else: self._status = SimpleActionState.INACTIVE # Grab goal key, if set if self._goal_key is not None: self._goal = ud[self._goal_key] # Write goal fields from userdata if set for key in self._goal_slots: setattr(self._goal,key,ud[key]) # Call user-supplied callback, if set, to get a goal if self._goal_cb is not None: try: goal_update = self._goal_cb( smach.Remapper( ud, self._goal_cb_input_keys, self._goal_cb_output_keys, []), self._goal, *self._goal_cb_args, **self._goal_cb_kwargs) if goal_update is not None: self._goal = goal_update except: rospy.logerr("Could not execute goal callback: "+traceback.format_exc()) return 'aborted' # Make sure the necessary paramters have been set if self._goal is None and self._goal_cb is None: rospy.logerr("Attempting to activate action "+self._action_name+" with no goal or goal callback set. Did you construct the SimpleActionState properly?") return 'aborted' # Dispatch goal via non-blocking call to action client self._activate_time = rospy.Time.now() self._status = SimpleActionState.ACTIVE # Wait on done condition self._done_cond.acquire() self._action_client.send_goal(self._goal, self._goal_done_cb, self._goal_active_cb, self._goal_feedback_cb) # Preempt timeout watch thread if self._exec_timeout: self._execution_timer_thread = threading.Thread(name=self._action_name+'/preempt_watchdog',target = self._execution_timer) self._execution_timer_thread.start() # Wait for action to finish self._done_cond.wait() # Call user result callback if defined result_cb_outcome = None if self._result_cb is not None: try: result_cb_outcome = self._result_cb( smach.Remapper( ud, self._result_cb_input_keys, self._result_cb_output_keys, []), self._goal_status, self._goal_result) if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes(): rospy.logerr("Result callback for action "+self._action_name+", "+str(self._result_cb)+" was not registered with the result_cb_outcomes argument. The result callback returned '"+str(result_cb_outcome)+"' but the only registered outcomes are: "+str(self.get_registered_outcomes())) return 'aborted' except: rospy.logerr("Could not execute result callback: "+traceback.format_exc()) return 'aborted' if self._result_key is not None: ud[self._result_key] = self._goal_result for key in self._result_slots: ud[key] = getattr(self._goal_result,key) # Check status if self._status == SimpleActionState.INACTIVE: # Set the outcome on the result state if self._goal_status == GoalStatus.SUCCEEDED: outcome = 'succeeded' elif self._goal_status == GoalStatus.PREEMPTED and self.preempt_requested(): outcome = 'preempted' self.service_preempt() else: # All failures at this level are captured by aborting, even if we timed out # This is an important distinction between local preemption, and preemption # from above. outcome = 'aborted' else: # We terminated without going inactive rospy.logwarn("Action state terminated without going inactive first.") outcome = 'aborted' # Check custom result cb outcome if result_cb_outcome is not None: outcome = result_cb_outcome # Set status inactive self._status = SimpleActionState.INACTIVE self._done_cond.release() return outcome ### Action client callbacks def _goal_active_cb(self): """Goal Active Callback This callback starts the timer that watches for the timeout specified for this action. """ rospy.logdebug("Action "+self._action_name+" has gone active.") def _goal_feedback_cb(self,feedback): """Goal Feedback Callback""" rospy.logdebug("Action "+self._action_name+" sent feedback.") def _goal_done_cb(self, result_state, result): """Goal Done Callback This callback resets the active flags and reports the duration of the action. Also, if the user has defined a result_cb, it is called here before the method returns. """ def get_result_str(i): strs = ('PENDING','ACTIVE','PREEMPTED','SUCCEEDED','ABORTED','REJECTED','LOST') if i <len(strs): return strs[i] else: return 'UNKNOWN ('+str(i)+')' # Calculate duration self._duration = rospy.Time.now() - self._activate_time rospy.logdebug("Action "+self._action_name+" terminated after "\ +str(self._duration.to_sec())+" seconds with result "\ +get_result_str(self._action_client.get_state())+".") # Store goal state self._goal_status = result_state self._goal_result = result # Rest status self._status = SimpleActionState.INACTIVE # Notify done self._done_cond.acquire() self._done_cond.notify() self._done_cond.release()
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, preempt_timeout = rospy.Duration(60.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 tobe 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 preempt_timeout: C{rospy.Duration} @param preempt_timeout: This is the timeout used for aborting after a preempt has been sent to the action and no result has been received. This timeout begins counting after a preempt message 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._exec_timeout = exec_timeout self._preempt_timeout = preempt_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") if not all([s in action_spec().action_goal.goal.__slots__ for s in goal_slots]): raise smach.InvalidStateError("Goal slots specified are not valid 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._preempt_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 # 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()
class NavStall(): 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 shutdown(self): self.cancelAndStop() ''' Implement leaky bucket for stall detection ''' def detect_stall(self, msg): now = rospy.Time.now() if msg.header.stamp.secs < (now.secs-1): #skip messages that are older than 1 sec (stale) return ''' if (self.hasGivenUp): self.givenUp() return #if we've been stalled for too long time, just give up if (self.stallCounterBucket > self.STOP_MAX_STALL_BUCKET_COUNT): self.hasGivenUp = 1 self.givenUp() return ''' if (msg.value > self.stall_current): self.stallCounterBucket+=2; rospy.loginfo("Potential stall condition detected at current: " + str(msg.value) + " (stall current: " + str(self.stall_current) + ") - incremented Stall Counter to " + str(self.stallCounterBucket)) else: if (self.stallCounterBucket > 0): self.stallCounterBucket-=1; #decrement bucket rospy.loginfo("Decremented Stall Counter to " + str(self.stallCounterBucket)) if (self.stallCounterBucket > self.MAX_STALL_BUCKET_COUNT): rospy.logwarn("Stall conditition detected! Trying to recover...") self.cancelAndStop() self.recover() rospy.logwarn("Stall recovery completed.") def recover(self): rospy.loginfo("Initiating recovery... Speed: " + str(self.recovery_speed) + " Time: " + str(self.recovery_time)) cmd_vel = Twist() #move back for one second at low speed cmd_vel.linear.x = -self.recovery_speed #need to repeat this multiple times, as base_controller will timeout after 0.5 sec for x in range(0, self.recovery_time*4): self.cmd_vel_pub.publish(cmd_vel) rospy.sleep(0.25) #stop rospy.loginfo("Stopping the robot after recovery move...") self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) def cancelAndStop(self): rospy.loginfo("Canceling all goals...") self.move_base.cancel_all_goals() rospy.sleep(1) rospy.loginfo("Stopping the robot...") self.cmd_vel_pub.publish(Twist()) rospy.sleep(1) '''
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!")
class AligningVehicle(smach.State): def __init__(self, resources): smach.State.__init__(self, outcomes=['aligned', 'timed_out'], input_keys=['time_out', 'e_x', 'e_y']) self.resources = resources 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 feedback_cb(self, feed): ##publish this error data to the sensor and move the vehicle msg = ipControllererror() msg.dy = feed.errorx msg.dx = msg.dy / tan(feed.errorangle) self.resources.ipControllerPublisher.publish(msg) ##done moving def done_cb(self, result): if result.sequence == header.MARKER_ALLIGNED: self.result = True else: self.result = False
class SimpleActionState(State): """Simple action client state. Use this class to represent an actionlib as a state in a state machine. """ # Meta-states for this action WAITING_FOR_SERVER = 0 INACTIVE = 1 ACTIVE = 2 PREEMPTING = 3 COMPLETED = 4 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 _wait_for_server(self): """Internal method for waiting for the action server This is run in a separate thread and allows construction of this state to not block the construction of other states. """ timeout_time = rospy.get_rostime() + self._server_wait_timeout while self._status == SimpleActionState.WAITING_FOR_SERVER and not rospy.is_shutdown( ) and not rospy.get_rostime() >= timeout_time: try: if self._action_client.wait_for_server( rospy.Duration(1.0)): #self._server_wait_timeout): self._status = SimpleActionState.INACTIVE if self.preempt_requested(): return except: if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called rospy.logerr("Failed to wait for action server '%s'" % (self._action_name)) def _execution_timer(self): """Internal method for cancelling a timed out goal after a timeout.""" while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown( ): try: rospy.sleep(0.1) except: if not rospy.is_shutdown(): rospy.logerr("Failed to sleep while running '%s'" % self._action_name) if rospy.Time.now() - self._activate_time > self._exec_timeout: goal = None if isinstance(self._goal, unicode): goal = self._goal.encode('utf8') else: goal = self._goal rospy.logwarn( "Action %s timed out after %d seconds. Cancelling goal: \n%s" % (self._action_name, self._exec_timeout.to_sec(), goal)) # Cancel the goal self.cancel_goal() break ### smach State API def request_preempt(self): rospy.loginfo("Preempt requested on action '%s'" % (self._action_name)) smach.State.request_preempt(self) if self._status == SimpleActionState.ACTIVE: goal = None if isinstance(self._goal, unicode): goal = self._goal.encode('utf8') else: goal = self._goal rospy.loginfo("Preempt on action '%s' cancelling goal: \n%s" % (self._action_name, goal)) # Cancel the goal self.cancel_goal() def cancel_goal(self): self._action_client.cancel_goal() self._cancel_time = rospy.Time.now() self._cancelation_timer_thread = threading.Thread( name=self._action_name + '/cancel_watchdog', target=self._cancelation_timer) self._cancelation_timer_thread.start() def _cancelation_timer(self): while self._status == SimpleActionState.ACTIVE and not rospy.is_shutdown( ): try: rospy.sleep(0.1) except: if not rospy.is_shutdown(): rospy.logerr("Failed to sleep while running '%s'" % self._action_name) if rospy.Time.now() - self._cancel_time > self._cancel_timeout: rospy.logerr( "Action %s could not be canceled for more than %d seconds. Force state transition!" % (self._action_name, self._cancel_timeout.to_sec())) self._status = SimpleActionState.INACTIVE self._done_cond.acquire() self._done_cond.notify() self._done_cond.release() def execute(self, ud): """Called when executing a state. This calls the goal_cb if it is defined, and then dispatches the goal with a non-blocking call to the action client. """ # Make sure we're connected to the action server if self._status is SimpleActionState.WAITING_FOR_SERVER: rospy.logwarn( "Still waiting for action server '%s' to start... is it running?" % self._action_name) if self._action_wait_thread.is_alive(): # Block on joining the server wait thread (This can be preempted) self._action_wait_thread.join() else: # Wait for the server in this thread (This can also be preempted) self._wait_for_server() if not self.preempt_requested(): # In case of preemption we probably didn't connect rospy.loginfo("Connected to action server '%s'." % self._action_name) # Check if server is still available if self._status is SimpleActionState.INACTIVE: try: if not self._action_client.wait_for_server( rospy.Duration(1.0)): rospy.logerr("Failed to wait for action server '%s'" % (self._action_name)) return 'aborted' except: if not rospy.core._in_shutdown: # This is a hack, wait_for_server should not throw an exception just because shutdown was called rospy.logerr("Failed to wait for action server '%s'" % (self._action_name)) return 'aborted' # Check for preemption before executing if self.preempt_requested(): rospy.loginfo("Preempting %s before sending goal." % self._action_name) self.service_preempt() return 'preempted' # We should only get here if we have connected to the server if self._status is SimpleActionState.WAITING_FOR_SERVER: rospy.logfatal("Action server for " + self._action_name + " is not running.") return 'aborted' else: self._status = SimpleActionState.INACTIVE # Grab goal key, if set if self._goal_key is not None: self._goal = ud[self._goal_key] # Write goal fields from userdata if set for key in self._goal_slots: setattr(self._goal, key, ud[key]) # Call user-supplied callback, if set, to get a goal if self._goal_cb is not None: try: goal_update = self._goal_cb( smach.Remapper(ud, self._goal_cb_input_keys, self._goal_cb_output_keys, []), self._goal, *self._goal_cb_args, **self._goal_cb_kwargs) if goal_update is not None: self._goal = goal_update except: rospy.logerr("Could not execute goal callback: " + traceback.format_exc()) return 'aborted' # Make sure the necessary paramters have been set if self._goal is None and self._goal_cb is None: rospy.logerr( "Attempting to activate action " + self._action_name + " with no goal or goal callback set. Did you construct the SimpleActionState properly?" ) return 'aborted' # Dispatch goal via non-blocking call to action client self._activate_time = rospy.Time.now() self._status = SimpleActionState.ACTIVE # Wait on done condition self._done_cond.acquire() self._action_client.send_goal(self._goal, self._goal_done_cb, self._goal_active_cb, self._goal_feedback_cb) # Preempt timeout watch thread if self._exec_timeout: self._execution_timer_thread = threading.Thread( name=self._action_name + '/preempt_watchdog', target=self._execution_timer) self._execution_timer_thread.start() # Wait for action to finish self._done_cond.wait() # Call user result callback if defined result_cb_outcome = None if self._result_cb is not None: try: result_cb_outcome = self._result_cb( smach.Remapper(ud, self._result_cb_input_keys, self._result_cb_output_keys, []), self._goal_status, self._goal_result) if result_cb_outcome is not None and result_cb_outcome not in self.get_registered_outcomes( ): rospy.logerr( "Result callback for action " + self._action_name + ", " + str(self._result_cb) + " was not registered with the result_cb_outcomes argument. The result callback returned '" + str(result_cb_outcome) + "' but the only registered outcomes are: " + str(self.get_registered_outcomes())) return 'aborted' except: rospy.logerr("Could not execute result callback: " + traceback.format_exc()) return 'aborted' if self._result_key is not None: ud[self._result_key] = self._goal_result for key in self._result_slots: ud[key] = getattr(self._goal_result, key) # Check status if self._status == SimpleActionState.INACTIVE: # Set the outcome on the result state if self._goal_status == GoalStatus.SUCCEEDED: outcome = 'succeeded' elif self._goal_status == GoalStatus.PREEMPTED and self.preempt_requested( ): outcome = 'preempted' self.service_preempt() else: # All failures at this level are captured by aborting, even if we timed out # This is an important distinction between local preemption, and preemption # from above. outcome = 'aborted' else: # We terminated without going inactive rospy.logwarn( "Action state terminated without going inactive first.") outcome = 'aborted' # Check custom result cb outcome if result_cb_outcome is not None: outcome = result_cb_outcome # Set status inactive self._status = SimpleActionState.INACTIVE self._done_cond.release() if self.preempt_requested(): outcome = 'preempted' self.service_preempt() return outcome ### Action client callbacks def _goal_active_cb(self): """Goal Active Callback This callback starts the timer that watches for the timeout specified for this action. """ rospy.logdebug("Action " + self._action_name + " has gone active.") def _goal_feedback_cb(self, feedback): """Goal Feedback Callback""" rospy.logdebug("Action " + self._action_name + " sent feedback.") def _goal_done_cb(self, result_state, result): """Goal Done Callback This callback resets the active flags and reports the duration of the action. Also, if the user has defined a result_cb, it is called here before the method returns. """ def get_result_str(i): strs = ('PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED', 'REJECTED', 'LOST') if i < len(strs): return strs[i] else: return 'UNKNOWN (' + str(i) + ')' # Calculate duration self._duration = rospy.Time.now() - self._activate_time rospy.logdebug("Action "+self._action_name+" terminated after "\ +str(self._duration.to_sec())+" seconds with result "\ +get_result_str(self._action_client.get_state())+".") # Store goal state self._goal_status = result_state self._goal_result = result # Rest status self._status = SimpleActionState.INACTIVE # Notify done self._done_cond.acquire() self._done_cond.notify() self._done_cond.release()
class Interaction(object): """ this is the entry point for interaction with outside resources to the state machine. it contains all the service proxies and servers, meant to be passed to all the states. gets rid of global variables. the boolean values are checked in states """ 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 positionCallback(self,msg): # self.pose.x=msg.data # self.pose.header=msg.header self.state=KrakenState() self.state.data=msg.data
class MeasurementsPub: 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 grid_cell(self, wx, wy): mx = int((wx - self.info.origin.position.x) / self.info.resolution) my = int((wy - self.info.origin.position.y) / self.info.resolution) return self.grid[mx, my] def load(self): try: with open(self.file_name, "r") as read_file: json_data = json.load(read_file) self.measurements = json_data["measurements"] self.goal = json_data["goal"] except Exception as e: print(e) def save(self): print("saving to", self.file_name) with open(self.file_name, "w") as write_file: json_data = {"measurements": self.measurements, "goal": self.goal} json.dump(json_data, write_file) def send_goal(self): x, y, d = self.goal if self.returning_home: return if y > self.origin[1] + self.height: self.return_home() return goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() heading = 0 if d == 1 else np.pi qx, qy, qz, qw = quaternion_from_euler(0, 0, heading) goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.x = qx goal.target_pose.pose.orientation.y = qy goal.target_pose.pose.orientation.z = qz goal.target_pose.pose.orientation.w = qw self.publish_area() # self.move_base.cancel_all_goals() self.move_base.send_goal(goal, self.goal_callback) def next_goal(self, goal): x, y, w = goal if (w == 1 and x + self.measure_dist > self.origin[0] + self.width ) or (w == -1 and x - self.measure_dist < self.origin[0]): y += self.measure_dist w *= -1 else: x += self.measure_dist * w if self.grid_cell(x, y) > 0: return self.next_goal((x, y, w)) else: return (x, y, w) def goal_callback(self, status, result): rospy.loginfo("Goal callback %s %s", status, result) x, y, w = self.goal if status == GoalStatus.SUCCEEDED: self.measurements.append({ "depth": self.depth, "timestamp": datetime.now().replace(microsecond=0).isoformat(), "x": x, "y": y }) self.points.append([x, y, self.depth]) header = Header(frame_id="map", stamp=rospy.Time.now()) msg = point_cloud2.create_cloud_xyz32(header, self.points) self.point_pub.publish(msg) self.goal = self.next_goal(self.goal) rospy.loginfo("Next goal %s", self.goal) self.send_goal() def service_next_goal(self, req): rospy.logwarn("Skipping goal") self.goal = self.next_goal(self.goal) rospy.loginfo("Next goal %s", self.goal) self.send_goal() return EmptyResponse() def return_home(self): rospy.loginfo("Returning home") goal = MoveBaseGoal() goal.target_pose.header.frame_id = "map" goal.target_pose.header.stamp = rospy.Time.now() x, y = self.gps_to_cell(self.home_lat, self.home_lon) print("x: ", x, "y: ", y) heading = 0 qx, qy, qz, qw = quaternion_from_euler(0, 0, heading) goal.target_pose.pose.position.x = x goal.target_pose.pose.position.y = y goal.target_pose.pose.orientation.x = qx goal.target_pose.pose.orientation.y = qy goal.target_pose.pose.orientation.z = qz goal.target_pose.pose.orientation.w = qw self.move_base.send_goal(goal) self.returning_home = True self.move_base.wait_for_result() def safety_callback(self, data): if data.battery <= 20: rospy.logerr("Battery low") self.return_home() if data.water == 1: rospy.logerr("Water in boat") self.return_home() #calculates cell from gps data def gps_to_cell(self, lat, lon): #calculate position in map frame if self.has_map == True and self.info.resolution != 0: #distance in metres between current position and bottom left corner of the map if not (lat < self.origin_lat or lon < self.origin_lon): dist_lat = vincenty((self.origin_lat, self.origin_lon), (lat, self.origin_lon)).m dist_lon = vincenty((self.origin_lat, self.origin_lon), (self.origin_lat, lon)).m x = dist_lon y = dist_lat return (x + self.info.origin.position.x, y + self.info.origin.position.y) else: return -1 #save depth at current position def depth_callback(self, data): self.depth = data.range def publish_area(self): msg = PolygonStamped() msg.header.frame_id = "map" msg.header.stamp = rospy.Time.now() origin_x, origin_y = self.origin msg.polygon.points = [ Point32(x, y, 0) for (x, y) in [(origin_x, origin_y), (origin_x + self.width, origin_y), (origin_x + self.width, origin_y + self.height), (origin_x, origin_y + self.height)] ] self.area_pub.publish(msg) #get map metadata def map_callback(self, data): self.info = data.info width = self.info.width height = self.info.height self.grid = np.matrix(np.array(data.data).reshape( (height, width))).transpose() self.has_map = True self.origin = self.gps_to_cell(self.start_lat, self.start_lon) print(self.origin) if not self.measurements: self.goal = (self.origin[0], self.origin[1], 1) self.send_goal()
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(): ## 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()))
class Interaction(object): """ this is the entry point for interaction with outside resources to the state machine. it contains all the service proxies and servers, meant to be passed to all the states. gets rid of global variables. the boolean values are checked in states """ 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=header.ESTIMATED_POSE_TOPIC_NAME,data_class= krakenPose , callback=self.positionCallback, queue_size=100) self.setPointController=SimpleActionClient(header.CONTROLLER_SERVER, controllerAction) rospy.loginfo("waiting for setPoint controller action Server") self.setPointController.wait_for_server() rospy.loginfo("Got controller server ") self.advancedControllerClient=SimpleActionClient(header.ADVANCED_CONTROLLER_SERVER, 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=header.IP_CONTROLLER_PUBLISHING_TOPIC, data_class=ipControllererror, queue_size=100) self.moveAlongService=rospy.ServiceProxy(name=header.MOVE_ALONG_SERVICE_NAME, 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=header.RESET_POSE_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=header.PREMAP_LOCATION_SERVICE, service_class=getLocation) rospy.loginfo("waiting for premap location Service") self.moveAlongService.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 positionCallback(self,msg): self.pose=msg.data self.pose.header=msg.header