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
def execute(self, ud): #comment this at last #self.resources=Interaction() #------------- rospy.loginfo("waiting for buoy_detect Server") buoyClient=SimpleActionClient(header.BUOY_DETECT_ACTION_SERVER, buoyAction) buoyClient.wait_for_server() rospy.loginfo("connected to server") goal=buoyGoal() goal.order=ip_header.ALLIGN_BUOY buoyClient.send_goal(goal, feedback_cb= self.feedback_cb) result=buoyClient.wait_for_result(rospy.Duration(ud.time_out)) if not result: buoyClient.cancel_goal() if buoyClient.get_state()==GoalStatus.SUCCEEDED: rospy.loginfo("successfully aligned the vehicle with the gate ") return 'aligned' else: rospy.logerr("buoy not allogned and timed out before alliging it!!!") return 'timed_out'
def execute(self, ud): #comment this at last self.resources=Interaction() #------------- ipClient=SimpleActionClient(header.MARKER_DETECT_ACTION_SERVER, markerAction) 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'
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()
class NavTest(): 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) def update_initial_pose(self, initial_pose): self.initial_pose = initial_pose def shutdown(self): rospy.loginfo("Stopping the robot...") self.move_base.cancel_goal() rospy.sleep(2) self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)
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))
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 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 Test(object): def __init__(self, action_server_name): # action server self.client = SimpleActionClient(action_server_name, ControllerListAction) self.client.wait_for_server() def send_cart_goal(self, goal_pose): goal = ControllerListGoal() goal.type = ControllerListGoal.STANDARD_CONTROLLER # 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 controller.weight = 1.0 goal.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 controller.weight = 1.0 goal.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_joint_goal(self, joint_state): goal = ControllerListGoal() goal.type = ControllerListGoal.STANDARD_CONTROLLER # translation controller = Controller() controller.type = Controller.JOINT controller.tip_link = 'gripper_tool_frame' controller.root_link = 'base_footprint' controller.goal_state = joint_state controller.p_gain = 3 controller.enable_error_threshold = False controller.threshold_value = 0.05 controller.weight = 1.0 goal.controllers.append(controller) self.client.send_goal(goal) result = self.client.wait_for_result(rospy.Duration(10)) print('finished in 10s?: {}'.format(result))
class NavTest(): 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) def update_initial_pose(self, initial_pose): self.initial_pose = initial_pose def shutdown(self): rospy.loginfo("Stopping the robot...") self.move_base.cancel_goal() rospy.sleep(2) self.cmd_vel_pub.publish(Twist()) rospy.sleep(1)