tianracer_competiton updated

tianracer_competition/launch/open_map_teb.launch

@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<launch>
+
+    <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen" clear_params="true">
+        <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+        <rosparam file="$(find tianracer_competition)/launch/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
+        <rosparam file="$(find tianracer_competition)/launch/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
+        <rosparam file="$(find tianracer_competition)/launch/param/local_costmap_params.yaml" command="load" />
+        <rosparam file="$(find tianracer_competition)/launch/param/global_costmap_params.yaml" command="load" />
+        <rosparam file="$(find tianracer_competition)/launch/param/move_base_params.yaml" command="load" />
+        <rosparam file="$(find tianracer_competition)/launch/param/teb_local_planner_params.yaml" command="load" />
+    </node>
+
+ <node name="nav_sim" pkg="tianracer_competition" type="nav_sim.py" />
+
+ <node name="loop" pkg="tianracer_competition" type="run3.py" />
+
+  <!-- Launch RVIZ -->
+    <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find tianracer_competition)/launch/sim.rviz" output="screen"/> -->
+</launch>

tianracer_competition/launch/param/costmap_common_params.yaml

@@ -0,0 +1,17 @@
+obstacle_range: 2.5 #只有障碍物在这个范围内才会被标记
+raytrace_range: 3.0 ##只有在这个范围内不存在的才会被消除
+# footprint: [[-0.08, -0.15], [-0.08, 0.15],[0.42, 0.15], [0.42, -0.15]]
+robot_radius: 0.165
+# inflation_radius: 0.2 #膨胀半径
+
+transform_tolerance: 0.5
+
+observation_sources: scan
+
+scan: 
+  data_type: LaserScan 
+  topic: scan 
+  marking: true 
+  clearing: true
+
+map_type: costmap

tianracer_competition/launch/param/global_costmap_params.yaml

@@ -0,0 +1,9 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  static_map: true
+  transform_tolerance: 0.5
+  cost_scaling_factor: 10.0
+  inflation_radius: 0.85

tianracer_competition/launch/param/local_costmap_params.yaml

@@ -0,0 +1,13 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 5
+  height: 5
+  resolution: 0.05
+  transform_tolerance: 0.5
+  cost_scaling_factor: 5
+  inflation_radius: 0.0

tianracer_competition/launch/param/move_base_params.yaml

@@ -0,0 +1,15 @@
+shutdown_costmaps: false #当move_base在不活动状态时,是否关掉costmap
+
+controller_frequency: 5.0 #向底盘控制移动话题cmd_vel发送命令的频率.
+controller_patience: 3.0
+
+planner_frequency: 0.5
+planner_patience: 5.0
+
+#机器人必须移动多远（以米计）才能被视为不摆动。
+#如果出现摆动则说明全局规划失败，那么将在超时后执行恢复模块
+oscillation_timeout: 10.0
+oscillation_distance: 0.1
+
+conservative_reset_dist: 0.1
+

tianracer_competition/launch/param/teb_local_planner_params.yaml

@@ -0,0 +1,127 @@
+TebLocalPlannerROS:
+
+  odom_topic: odom
+  map_frame: /map
+
+  # Trajectoty 这部分主要是用于调整轨迹
+
+  teb_autosize: True #优化期间允许改变轨迹的时域长度
+  dt_ref: 0.3 #期望的轨迹时间分辨率
+  dt_hysteresis: 0.03 #根据当前时间分辨率自动调整大小的滞后现象，通常约为。建议使用dt ref的10%
+
+  #覆盖全局规划器提供的局部子目标的方向;规划局部路径时会覆盖掉全局路径点的方位角,
+  #对于车辆的2D规划,可以设置为False,可实现对全局路径的更好跟踪。
+  global_plan_overwrite_orientation: True
+
+#指定考虑优化的全局计划子集的最大长度,如果为0或负数：禁用；长度也受本地Costmap大小的限制
+  max_global_plan_lookahead_dist: 3.0 
+
+  feasibility_check_no_poses: 1 #检测位姿可到达的时间间隔，default：4
+
+  #如果为true，则在目标落后于起点的情况下，可以使用向后运动来初始化基础轨迹
+  #(仅在机器人配备了后部传感器的情况下才建议这样做）
+  allow_init_with_backwards_motion: False
+
+  global_plan_viapoint_sep: -1
+
+  #参数在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用
+  #该参数决定了从机器人当前位置的后面一定距离开始裁剪
+  #就是把机器人走过的全局路线给裁剪掉，因为已经过去了没有比较再参与计算后面的局部规划
+  global_plan_prune_distance: 1
+
+  exact_arc_length: False
+  publish_feedback: False
+
+  # Robot
+  max_vel_x: 2.5
+  max_vel_x_backwards: 1
+  max_vel_theta: 3.5
+  acc_lim_x: 2.5
+  acc_lim_theta: 3.5
+
+  #仅适用于全向轮
+  # max_vel_y (double, default: 0.0)  
+  # acc_lim_y (double, default: 0.5)
+
+  # ********************** Carlike robot parameters ********************
+  min_turning_radius: 0.20     # 最小转弯半径 注意车辆运动学中心是后轮中点
+  wheelbase: 0.33                 # 即前后轮距离
+
+#设置为true时，ROS话题（rostopic） cmd_vel/angular/z 内的数据是舵机角度，
+  cmd_angle_instead_rotvel: True 
+  # ********************************************************************
+
+  # footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 多边形勿重复第一个顶点，会自动闭合
+  #   type: "line"
+  #   # radius: 0.2 # for type "circular"
+  #   line_start: [-0.13, 0.0] # for type "line"
+  #   line_end: [0.13, 0.0] # for type "line"
+    # front_offset: 0.2 # for type "two_circles"
+    # front_radius: 0.2 # for type "two_circles"
+    # rear_offset: 0.2 # for type "two_circles"
+    # rear_radius: 0.2 # for type "two_circles"
+    # vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
+
+  # GoalTolerance
+  footprint_model:
+    type: "polygon"
+    vertices: [[-0.165, -0.165], [-0.165, 0.165], [0.165, 0.165], [0.165, -0.165]]    
+
+  xy_goal_tolerance: 0.1
+  yaw_goal_tolerance: 6.28
+  #自由目标速度。设为False时，车辆到达终点时的目标速度为0。
+  #TEB是时间最优规划器。缺少目标速度约束将导致车辆“全速冲线”
+  free_goal_vel: True
+
+  # complete_global_plan: True   
+  # Obstacles
+
+  min_obstacle_dist: 0.1 # 与障碍的最小期望距离,
+  include_costmap_obstacles: True #应否考虑到局部costmap的障碍设置为True后才能规避实时探测到的、建图时不存在的障碍物。
+  costmap_obstacles_behind_robot_dist: 3.0 #考虑后面n米内的障碍物2.0
+  obstacle_poses_affected: 30 #为了保持距离，每个障碍物位置都与轨道上最近的位置相连。
+  costmap_converter_spin_thread: True
+  costmap_converter_rate: 5
+
+  # Optimization
+
+  no_inner_iterations: 5
+  no_outer_iterations: 4
+  optimization_activate: True
+  optimization_verbose: False
+  penalty_epsilon: 0.1
+  weight_max_vel_x: 2
+  weight_max_vel_theta: 1
+  weight_acc_lim_x: 1
+  weight_acc_lim_theta: 2 #1
+  weight_kinematics_nh: 1000
+  weight_kinematics_forward_drive: 0.1 #1
+  weight_kinematics_turning_radius: 1 #1
+  weight_optimaltime: 1
+  weight_obstacle: 10 #50
+  weight_dynamic_obstacle: 10 # not in use yet
+  alternative_time_cost: False # not in use yet
+  selection_alternative_time_cost: False
+  # Homotopy Class Planner
+
+  enable_homotopy_class_planning: False
+  enable_multithreading: False
+  simple_exploration: False
+  max_number_classes: 4
+  roadmap_graph_no_samples: 15
+  roadmap_graph_area_width: 5
+  h_signature_prescaler: 0.5
+  h_signature_threshold: 0.1
+  obstacle_keypoint_offset: 0.1
+  obstacle_heading_threshold: 0.45
+  visualize_hc_graph: False
+
+  # # Recovery
+
+  # shrink_horizon_backup: True
+  # shrink_horizon_min_duration: 10
+  # oscillation_recovery: True
+  # oscillation_v_eps: 0.1
+  # oscillation_omega_eps: 0.1
+  # oscillation_recovery_min_duration: 10
+  # oscillation_filter_duration: 10

tianracer_competition/script/nav_sim.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+import rospy
+import std_msgs.msg
+from ackermann_msgs.msg import AckermannDriveStamped
+from geometry_msgs.msg import Twist
+
+import time 
+import threading
+pub = rospy.Publisher("/drive", AckermannDriveStamped,queue_size=1)
+
+def thread_job():
+    rospy.spin()
+
+def callback(data):
+    speed = data.linear.x 
+    turn = data.angular.z
+
+    msg = AckermannDriveStamped();
+    msg.header.stamp = rospy.Time.now();
+    msg.header.frame_id = "base_link";
+
+    msg.drive.speed = speed;
+    msg.drive.acceleration = 1;
+    msg.drive.jerk = 1;
+    msg.drive.steering_angle = turn
+    msg.drive.steering_angle_velocity = 1
+
+    pub.publish(msg)
+
+def SubscribeAndPublish():
+    rospy.init_node('nav_sim', anonymous=True)
+    rospy.Subscriber('cmd_vel', Twist, callback,queue_size=1,buff_size=52428800)
+    #rospy.Subscriber('cmd_vel', Twist, callback,queue_size=1,buff_size=52428800)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    try:
+        SubscribeAndPublish()
+    except rospy.ROSInterruptException:
+        pass
+
+
+########################

tianracer_competition/script/run3.py

@@ -0,0 +1,131 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import roslib
+import rospy
+import actionlib
+from actionlib_msgs.msg import *
+from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+from nav_msgs.msg import Odometry
+
+x = 0
+
+def callback(data):
+    global x
+    x=data.pose.pose.position.x
+    # print(x)
+
+
+    # y=data.pose.pose.position.y
+
+
+
+def move3():
+
+    # 设定目标点
+    target = Pose(Point(-7, 0, 0.000), Quaternion(0.000, 0.000,-0.0015, 0.99))
+    goal = MoveBaseGoal()
+    goal.target_pose.pose = target
+    goal.target_pose.header.frame_id = 'map'
+    goal.target_pose.header.stamp = rospy.Time.now()
+
+    rospy.loginfo("Going to: " + str(target))
+
+
+    # 向目标进发
+    move_base.send_goal(goal)
+
+    # move_base.wait_for_result()
+
+    while x>-12.2:
+        # print(x)
+        pass
+    # print('a')
+    # move_base.cancel_goal()
+    move1()
+
+
+
+
+
+def move2():
+
+    # 设定目标点
+    target = Pose(Point(-7, 8.7, 0.000), Quaternion(0.000, 0.000,0.99, 0.0016))
+    goal = MoveBaseGoal()
+    goal.target_pose.pose = target
+    goal.target_pose.header.frame_id = 'map'
+    goal.target_pose.header.stamp = rospy.Time.now()
+
+    rospy.loginfo("Going to: " + str(target))
+
+
+    # 向目标进发
+    move_base.send_goal(goal)
+
+    # move_base.wait_for_result()
+
+    while x>-4.5:
+        # print(x)
+        pass
+    print('a')
+    # move_base.cancel_goal()
+    move3()
+    print('b')
+
+
+
+
+def move1():
+
+    # 设定目标点
+    target = Pose(Point(7, 0, 0.000), Quaternion(0.000, 0.000,-0.0015, 0.99))
+    goal = MoveBaseGoal()
+    goal.target_pose.pose = target
+    goal.target_pose.header.frame_id = 'map'
+    goal.target_pose.header.stamp = rospy.Time.now()
+
+    rospy.loginfo("Going to: " + str(target))
+
+    # 向目标进发
+    move_base.send_goal(goal)
+
+    # move_base.wait_for_result()
+
+    while x<3.5:
+        # print(x)
+        pass
+    print('a')
+    # move_base.cancel_goal()
+    move2()
+    print('b')
+
+
+
+
+
+
+
+
+
+
+if __name__ == '__main__':
+    # 节点初始化
+    rospy.init_node('move_test', anonymous=True)
+
+    # 订阅move_base服务器的消息
+    move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
+    rospy.Subscriber('odom', Odometry, callback,queue_size=1,buff_size=52428800)
+
+    rospy.loginfo("Waiting for move_base action server...")
+
+    # 等待连接服务器，5s等待时间限制
+    while move_base.wait_for_server(rospy.Duration(5.0)) == 0:
+        rospy.loginfo("Connected to move base server")
+
+    for i in range(0,100):
+        move1()
+
+
+

tianracer_competition/script/run_in_closed_map.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+#coding=utf-8
 # created by Wu GengQian
 
 import rospy

tianracer_competition/script/run_in_open_map.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python
+#coding=utf-8
 # created by Wu GengQian
 
 import rospy