Navigation

CMakeLists.txt

@@ -49,6 +49,7 @@ if(DEFINED CATKIN_DEVEL_PREFIX)
     nav_msgs
     sensor_msgs
     geometry_msgs
+    move_base
     )
 
   # Include catkin_libraries

launch/map_robot.rviz

@@ -0,0 +1,242 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map2
+        - /Path1
+      Splitter Ratio: 0.5
+    Tree Height: 761
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        back_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        back_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_left_hinge:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        front_right_hinge:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        front_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser_model:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 255; 0; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/TrajectoryPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/TrajectoryPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 20.865917205810547
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: -6.641196250915527
+        Y: 1.858250379562378
+        Z: 4.814024448394775
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.1847965717315674
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 5.95858097076416
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1052
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000382fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000382000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000382fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000382000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007490000003efc0100000002fb0000000800540069006d00650100000000000007490000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004d80000038200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1865
+  X: 55
+  Y: 0

launch/simulate.launch

@@ -5,12 +5,31 @@
   <!-- Launch a map from the maps folder-->
   <arg name="map" default="$(find racecar_simulator)/maps/building_31.yaml"/>
   <node pkg="map_server" name="map_server" type="map_server" args="$(arg map)"/>
-
+  
   <!-- Launch the racecar model -->
   <include file="$(find racecar_simulator)/launch/racecar_model.launch"/>
 
   <!-- Begin the simulator with the parameters from params.yaml -->
   <node pkg="racecar_simulator" name="racecar_simulator" type="simulate" output="screen">
     <rosparam command="load" file="$(find racecar_simulator)/params.yaml"/>
   </node>
+
+  <!-- Launch the amcl node for tracking position -->
+  <include file="$(find amcl)/examples/amcl_diff.launch" />
+
+  <!-- Launch the move base planner to plan a trajectory -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find racecar_simulator)/params/costmap_common_config.yaml" command="load" ns="global_costmap" /> 
+    <rosparam file="$(find racecar_simulator)/params/costmap_common_config.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find racecar_simulator)/params/local_costmap_param.yaml" command="load" />
+    <rosparam file="$(find racecar_simulator)/params/global_costmap_param.yaml" command="load" /> 
+    <rosparam file="$(find racecar_simulator)/params/base_local_planner.yaml" command="load" />
+  </node>
+
+  <!-- Launch the bridge for convertng Twist to AckermannDrive -->
+  <node pkg="racecar_simulator" name="twist_to_ackermann" type="twist_to_ackermann.py" output="screen"/>
+
+  <!-- Launch rviz for visualization -->
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find racecar_simulator)/launch/map_robot.rviz" />
+
 </launch>

node/simulate.cpp

@@ -19,7 +19,7 @@ using namespace racecar_simulator;
 class RacecarSimulator {
   private:
     // The transformation frames used
-    std::string map_frame, base_frame, scan_frame;
+    std::string map_frame, base_frame, scan_frame, odom_frame;
 
     // The car state and parameters
     Pose2D pose;
@@ -44,6 +44,9 @@ class RacecarSimulator {
     // For publishing transformations
     tf2_ros::TransformBroadcaster br;
 
+    // For publishing odom
+    tf2_ros::TransformBroadcaster br_odom;
+
     // A timer to update the pose
     ros::Timer update_pose_timer;
 
@@ -91,6 +94,7 @@ class RacecarSimulator {
       n.getParam("map_frame", map_frame);
       n.getParam("base_frame", base_frame);
       n.getParam("scan_frame", scan_frame);
+      n.getParam("odom_frame", odom_frame);
 
       // Fetch the car parameters
       int scan_beams;
@@ -170,17 +174,37 @@ class RacecarSimulator {
       t.rotation.z = quat.z();
       t.rotation.w = quat.w();
 
-      // Add a header to the transformation
-      geometry_msgs::TransformStamped ts;
-      ts.transform = t;
-      ts.header.stamp = timestamp;
-      ts.header.frame_id = map_frame;
-      ts.child_frame_id = base_frame;
+      // Publish a map/odom transformation
+      geometry_msgs::TransformStamped map_to_odom;
+      map_to_odom.header.stamp = timestamp;
+      map_to_odom.transform.translation.x = 0.0;
+      map_to_odom.transform.translation.y = 0.0;
+      map_to_odom.transform.translation.z = 0.0;
+      map_to_odom.transform.rotation.x = 0.0;
+      map_to_odom.transform.rotation.y = 0.0;
+      map_to_odom.transform.rotation.z = 0.0;
+      map_to_odom.transform.rotation.w = 1.0;
+      map_to_odom.header.frame_id = map_frame;
+      map_to_odom.child_frame_id = odom_frame;
+
+      // Publish an odom/base_link transformation
+      geometry_msgs::TransformStamped odom_trans;
+      odom_trans.header.stamp = timestamp;
+      odom_trans.header.frame_id = odom_frame;
+      odom_trans.child_frame_id = base_frame;
+
+      odom_trans.transform.translation.x = pose.x;
+      odom_trans.transform.translation.y = pose.y;
+      odom_trans.transform.translation.z = 0.0;
+      odom_trans.transform.rotation.x = quat.x();
+      odom_trans.transform.rotation.y = quat.y();
+      odom_trans.transform.rotation.z = quat.z();
+      odom_trans.transform.rotation.w = quat.w();
 
       // Make an odom message as well
       nav_msgs::Odometry odom;
       odom.header.stamp = timestamp;
-      odom.header.frame_id = map_frame;
+      odom.header.frame_id = odom_frame;
       odom.child_frame_id = base_frame;
       odom.pose.pose.position.x = pose.x;
       odom.pose.pose.position.y = pose.y;
@@ -193,8 +217,9 @@ class RacecarSimulator {
         AckermannKinematics::angular_velocity(speed, steering_angle, wheelbase);
 
       // Publish them
-      if (broadcast_transform) br.sendTransform(ts);
+      if (broadcast_transform) br.sendTransform(map_to_odom);
       odom_pub.publish(odom);
+      if (broadcast_transform) br_odom.sendTransform(odom_trans);
       // Set the steering angle to make the wheels move
       set_steering_angle(steering_angle, timestamp);
 

package.xml

@@ -16,6 +16,7 @@
   <build_depend>nav_msgs</build_depend>
   <build_depend>sensor_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
+  <build_depend>move_base</build_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>tf2</run_depend>
   <run_depend>tf2_ros</run_depend>
@@ -26,4 +27,5 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>joy</run_depend>
   <run_depend>map_server</run_depend>
+  <run_depend>move_base</run_depend>
 </package>

params.yaml

@@ -46,6 +46,7 @@ pose_rviz_topic: "/initialpose"
 map_frame: "map"
 base_frame: "base_link"
 scan_frame: "laser"
+odom_frame: "odom"  
 
 # Turn this off for mapping
 broadcast_transform: true