How to use make_plan method in tappy

Best Python code snippet using tappy_python

relay.py

Source:relay.py

1#!/usr/bin/env python2import actionlib3import copy4import rospy5import nav_msgs.srv as nav_srvs6import mbf_msgs.msg as mbf_msgs7import move_base_msgs.msg as mb_msgs8from dynamic_reconfigure.client import Client9from dynamic_reconfigure.server import Server10from geometry_msgs.msg import PoseStamped11"""12move_base legacy relay node:13Relays old move_base actions to the new mbf move_base action, similar but with richer result and feedback.14We also relay the simple goal topic published by RViz, the make_plan service and dynamic reconfiguration15calls (note that some parameters have changed names; see http://wiki.ros.org/move_base_flex for details)16"""17# keep configured base local and global planners to send to MBF18bgp = None19blp = None20def simple_goal_cb(msg):21    mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg, planner=bgp, controller=blp))22    rospy.logdebug("Relaying move_base_simple/goal pose to mbf")23def mb_execute_cb(msg):24    mbf_mb_ac.send_goal(mbf_msgs.MoveBaseGoal(target_pose=msg.target_pose, planner=bgp, controller=blp),25                        feedback_cb=mbf_feedback_cb)26    rospy.logdebug("Relaying legacy move_base goal to mbf")27    mbf_mb_ac.wait_for_result()28    status = mbf_mb_ac.get_state()29    result = mbf_mb_ac.get_result()30    rospy.logdebug("MBF execution completed with result [%d]: %s", result.outcome, result.message)31    if result.outcome == mbf_msgs.MoveBaseResult.SUCCESS:32        mb_as.set_succeeded(mb_msgs.MoveBaseResult(), "Goal reached.")33    else:34        mb_as.set_aborted(mb_msgs.MoveBaseResult(), result.message)35def make_plan_cb(request):36    mbf_gp_ac.send_goal(mbf_msgs.GetPathGoal(start_pose=request.start, target_pose=request.goal,37                                             use_start_pose=bool(request.start.header.frame_id),38                                             planner=bgp, tolerance=request.tolerance))39    rospy.logdebug("Relaying legacy make_plan service to mbf get_path action server")40    mbf_gp_ac.wait_for_result()41    status = mbf_gp_ac.get_state()42    result = mbf_gp_ac.get_result()43    rospy.logdebug("MBF get_path execution completed with result [%d]: %s", result.outcome, result.message)44    if result.outcome == mbf_msgs.GetPathResult.SUCCESS:45        return nav_srvs.GetPlanResponse(plan=result.path)46def mbf_feedback_cb(feedback):47    mb_as.publish_feedback(mb_msgs.MoveBaseFeedback(base_position=feedback.current_pose))48if __name__ == '__main__':49    rospy.init_node("move_base")50    # TODO what happens with malformed target goal???  FAILURE  or INVALID_POSE51    # txt must be:  "Aborting on goal because it was sent with an invalid quaternion"   52    # move_base_flex get_path and move_base action clients53    mbf_mb_ac = actionlib.SimpleActionClient("move_base_flex/move_base", mbf_msgs.MoveBaseAction)54    mbf_gp_ac = actionlib.SimpleActionClient("move_base_flex/get_path", mbf_msgs.GetPathAction)55    mbf_mb_ac.wait_for_server(rospy.Duration(20))56    mbf_gp_ac.wait_for_server(rospy.Duration(10))57    # move_base simple topic and action server58    mb_sg = rospy.Subscriber('move_base_simple/goal', PoseStamped, simple_goal_cb)59    mb_as = actionlib.SimpleActionServer('move_base', mb_msgs.MoveBaseAction, mb_execute_cb, auto_start=False)60    mb_as.start()61    # move_base make_plan service62    mb_mps = rospy.Service('~make_plan', nav_srvs.GetPlan, make_plan_cb)...

s624795956.py

Source:s624795956.py

...11        if pi:12            score = (i+1) * 10013            total += score * pi + bonus[i]14    return total15def make_plan(total_problem, total_score, flag):16    """17    :param total_score: ããã¾ã§ã®åè¨ã¹ã³ã¢18    :param total_problem: ããã¾ã§ã®åè¨åé¡æ°19    :param flag: æå¾ã®ã¹ã³ã¢èª¿æ´å®æ½æ¸ã¿ãã©ã°20    :return: åè¨åé¡æ°21    """22    if total_score >= G:23        return total_problem24    # print("call ", total_problem, total_score, problems)25    rest_score = calc_rest_score()  # æ®ãå¨é¨è§£ãããä½ç¹ã«ãªãã®ã26    if total_score + rest_score < G:27        # ããç¡ç28        return 1000029    for i in range(N - 1, -1, -1):30        pi = problems[i]31        si = (i + 1) * 10032        if pi == 0:33            continue34        else:35            # 1. å¨é¨è§£ã36            problems[i] = 037            pn1 = make_plan(total_problem + pi,38                            total_score + si * pi + bonus[i],39                            flag)40            problems[i] = pi41            # print("pn1:", pn1)42            # 2. é¨åçã«è§£ã(ãã ãæå¾ã®èª¿æ´ã¨ãã¦)43            if not flag:44                # 1åãã¤æ¸ããã¦çµæãæ¯è¼45                min_pn = 10000  # å¤ååé¡æ°ã¯maxã§ã5500åããã46                for j in range(pi, -1, -1):  # 1ï½(pi-1)47                    plus_score = si * j48                    if j == pi:49                        plus_score += bonus[i]50                    problems[i] = 051                    pn = make_plan(total_problem+j,52                                   total_score+plus_score,53                                   True if j != 0 else flag)54                    # print("test", j, pn)55                    problems[i] = pi56                    if pn < min_pn:57                        min_pn = pn58                pn2 = min_pn59            else:60                pn2 = 1000061            return min(pn1, pn2)62    else:63        # åé¡ãé¿ãããã¦è§£ããªã64        return 10000...

make_plan.py

Source:make_plan.py

...23            goal = PoseStamped(Header(0, ctime, "map"),24                                Pose(Point(target_x, target_y, 0), 25                                    Quaternion(0,0,0,1)))26            tolerance = 027            resp = make_plan(start, goal, tolerance)28            return resp.plan.poses29        except rospy.ServiceException as e:30            print("Service call failed: %s"%e)31if __name__ == "__main__":32    rospy.init_node("make_plan")33    # program = MakePlan()34    # print(len(program.get_plan()))...

Automation Testing Tutorials

Learn to execute automation testing from scratch with LambdaTest Learning Hub. Right from setting up the prerequisites to run your first automation test, to following best practices and diving deeper into advanced test scenarios. LambdaTest Learning Hubs compile a list of step-by-step guides to help you be proficient with different test automation frameworks i.e. Selenium, Cypress, TestNG etc.