simple turtlebot tutorial

Author: micahpearlman

Documentation~/images/novnc_login.png

@@ -47,6 +47,11 @@ ZeroSim provides a multitude of tools for building robots and environments in Un
 
 ## Getting Started
 
+### Recommended System
+
+* Ubuntu 18.04 or 20.04 (may work on MacOS or Windows but currently untested)
+* Unity 2020.x
+
 ### Setting up a new Unity Project
 
 1. In Unity Hub create a new Unity project using Unity 2020.x or later. ![New Unity Project](Documentation~/images/new_unity_project.png)
@@ -61,4 +66,45 @@ ZeroSim provides a multitude of tools for building robots and environments in Un
 3. If running Unity on Linux you want to avoid using OpenGL and use Vulkan, otherwise image based sensors may run slowly or not at all.  To change to using Vulkan:  
    1. In the Unity Menu: `Edit -> Project Settings...`:  
    2. Uncheck `Auto Graphics API for Linux` and then under `Graphics APIs for Linux` set `Vulkan` ahead of `OpenGL`:  
-   ![Vulkan Settings](Documentation~/images/vulkan_settings.png) 
+   ![Vulkan Settings](Documentation~/images/vulkan_settings.png) 
+
+
+### Getting ZeroSim ROS Docker Container
+
+1.  Available at https://hub.docker.com/r/zerodog/zerosim_ros_vnc or `docker pull zerodog/zerosim_ros_vnc:latest`
+### Running TurtleBot Test Scene
+
+*NOTE:* Order of operations is important.  Especially starting the Docker *before* the Unity simulation.
+
+1. Make sure that the ZeroSim samples are installed as outlined above.
+2. Make sure that the ZeroSim Docker container above is installed.
+3. Open the `Scenes/Turtlebot3_Waffle_test.scene`
+4. Launch the ZeroSim Docker via: 
+```
+docker run -it --rm \
+--publish=9090:9090 \
+--publish=11311:11311 \
+--publish=8083:8083 \
+--publish=80:80 \
+--publish=5678:5678 \
+--name my_zerosim_vnc_docker \
+zerodog/zerosim_ros_vnc:latest \
+roslaunch zero_sim_ros basic_unity_editor.launch
+
+```
+5. Run the ROS teleop in a seperate terminal by running: 
+```
+docker exec -it my_zerosim_vnc_docker \
+bash -c "source devel/setup.bash ; rosrun turtlebot3_teleop turtlebot3_teleop_key"
+```
+6. In the Unity editor press the "Play" button.
+7. The Turtlebot can now be controlled via the `w a s d` keys in the ROS teleop console window:
+
+### Using RViz Example
+
+1. Startup the Turtlebot Test Scene as detailed above.
+2. Open a noVNC connection by:
+   1. In a browser open http://localhost:8083/vnc.html
+   2. Press the "Connect" button. ![noVNC Login](Documentation~/images/novnc_login.png)
+3. In the VNC window press the *LEFT* mouse button and select "Terminal". ![noVNC Terminal](Documentation~/images/novnc_terminal.png)
+4. In the new terminal run `rviz -d ./src/zero_sim_ros/rviz/turtlebot_viewer.rviz`.  RViz will start up with a 3D view with the LIDAR scanner visibile. ![RViz Turtlebot Viewer](Documentation~/images/rviz_turtlebot.gif)

Documentation~/images/novnc_terminal.png

@@ -181,25 +181,26 @@ protected override void ZOFixedUpdateHzSynchronized() {
                 if (GoalStatus == ActionStatusEnum.ACTIVE) {
 
                     // find the closest target position given the current time
-                    bool foundTargetPoint = false;
-                    foreach (JointTrajectoryPointMessage point in Goal.goal.trajectory.points) {
-                        // Debug.Log("INFO: point time: " + point.time_from_start.Seconds.ToString("R3"));
-                        if (_currentGoalTime <= point.time_from_start.Seconds) {
-                            _currentPoint = point;
-                            foundTargetPoint = true;
-                            break;
-                        }
-                    }
-
-
-                    if (foundTargetPoint == true) {
+                    // bool foundTargetPoint = false;
+                    // foreach (JointTrajectoryPointMessage point in Goal.goal.trajectory.points) {
+                    //     // Debug.Log("INFO: point time: " + point.time_from_start.Seconds.ToString("R3"));
+                    //     if (_currentGoalTime <= point.time_from_start.Seconds) {
+                    //         _currentPoint = point;
+                    //         foundTargetPoint = true;
+                    //         break;
+                    //     }
+                    // }
+
+                    double[] interpolatedJointPositions = GetJointPositionsAtTimeSeconds(Goal.goal.trajectory.points, _currentGoalTime);
+                    if (interpolatedJointPositions != null) {
+                        
 
                         for (int i = 0; i < Goal.goal.trajectory.joint_names.Length; i++) {
                             _trajectoryControllerStateMessage.joint_names[i] = Goal.goal.trajectory.joint_names[i];
                             ZOJointInterface joint = GetJointByName(Goal.goal.trajectory.joint_names[i]);
 
-                            _trajectoryControllerStateMessage.desired.positions[i] = _currentPoint.positions[i];
-                            ActionMessage.action_feedback.feedback.desired.positions[i] = _currentPoint.positions[i];
+                            _trajectoryControllerStateMessage.desired.positions[i] = interpolatedJointPositions[i];
+                            ActionMessage.action_feedback.feedback.desired.positions[i] = interpolatedJointPositions[i];
 
                             _trajectoryControllerStateMessage.actual.positions[i] = joint.Position;
                             ActionMessage.action_feedback.feedback.actual.positions[i] = joint.Position;
@@ -301,6 +302,41 @@ protected override void ZOOnGUI() {
         }
         #endregion // ZOGameObjectBase
 
+        #region Utils
+
+        protected double[] GetJointPositionsAtTimeSeconds(JointTrajectoryPointMessage[] points, float timeSeconds) {
+
+            if (points.Length > 1) {
+                for (int i = 1; i < points.Length; i++) {
+                    // search for bracket
+                    double aTime = points[i - 1].time_from_start.Seconds;
+                    double bTime = points[i].time_from_start.Seconds;                    
+                    if (timeSeconds > aTime && timeSeconds <= bTime) {
+                        
+                        double timeBetweenPoints = bTime - aTime;
+                        if (timeBetweenPoints > 0) {
+                            double timeFromA = timeSeconds - aTime;
+                            double t = timeFromA / timeBetweenPoints;
+                            int numPositions = points[i].positions.Length;
+                            double[] interpolatedJointPositions = new double[numPositions];
+                            for (int p = 0; p < numPositions; p++) {
+                                double aJointPosition = points[i-1].positions[p];
+                                double bJointPosition = points[i].positions[p];
+                                interpolatedJointPositions[p] = Mathf.LerpAngle((float)aJointPosition * Mathf.Rad2Deg, (float)bJointPosition * Mathf.Deg2Rad, (float)t) * Mathf.Deg2Rad;
+                            }
+
+                            return interpolatedJointPositions;
+
+                        }
+                    }
+                }
+            }
+
+            return null; // failed to find joint positions bracket.  perhaps time has gone beyond the last joint position.
+        }
+
+        #endregion // Utils
+
         #region ZOROSControllerInterface
 
         public string ControllerName {
@@ -363,6 +399,11 @@ public string[] JointNames {
             }
         }
 
+        /// <summary>
+        /// Get Joint given its string name
+        /// </summary>
+        /// <param name="name">The name of the joint</param>
+        /// <returns>ZOJointInterface</returns>
         public ZOJointInterface GetJointByName(string name) {
             foreach (ZOJointInterface joint in Joints) {
                 if (joint.Name == name) {

Documentation~/images/rviz_turtlebot.gif

@@ -7,8 +7,8 @@ PhysicMaterial:
   m_PrefabInstance: {fileID: 0}
   m_PrefabAsset: {fileID: 0}
   m_Name: StickyNoBounce
-  dynamicFriction: 1
-  staticFriction: 1
+  dynamicFriction: 2
+  staticFriction: 2
   bounciness: 0
   frictionCombine: 0
   bounceCombine: 0