WIP #9: Wrote ROS node for simulating robot's motion

simulation_tools/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>simulation_tools</name>
+  <version>0.0.0</version>
+  <description>The simulation_tools package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ekaj@todo.todo">ekaj</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/simulation_tools</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>geometry_msgs</build_export_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>geometry_msgs</exec_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

simulation_tools/scripts/path_tracking_sim.py

@@ -0,0 +1,126 @@
+#!/usr/bin/env python
+
+# ----------------------------- #
+# Subscribes to drive command, Publishes custom motor speeds. #
+# ----------------------------- #
+
+# Import ROS packages
+import rospy
+from geometry_msgs.msg import Twist, Pose
+
+# Import other required packages
+from math import sin, cos, pi
+import numpy as np
+import cv2
+
+class Robot:
+
+    def __init__(self, twist_topic, pose_topic, init_pose=Pose()):
+
+        # True state of robot
+        self.pose = init_pose
+        self.desired_lin_vel = 1.0
+        self.desired_ang_vel = -2.12132034356
+
+        # Subscriber to listen for incoming drive commands
+        self.twist_sub = rospy.Subscriber(twist_topic, Twist, self.set_twist)
+
+        # Publisher to send pose estimates
+        self.pose_pub = rospy.Publisher(pose_topic, Pose, queue_size=0)
+
+    # Sets the robot's desired velocities from the incoming message
+    def set_twist(self, new_twist_msg):
+        self.desired_lin_vel = new_twist_msg.linear.x
+        self.desired_ang_vel = new_twist_msg.angular.x
+
+    # Computes new pose after moving at current twist for the specified time
+    def move_for_time(self, sec):
+
+        # Compute actual velocities for this time-step
+        noisy_lin_vel = self.desired_lin_vel # + noise
+        noisy_ang_vel = self.desired_ang_vel # + noise
+
+        # Compute new position and orientation
+        init_theta = self.pose.orientation.x
+        final_theta = self.pose.orientation.x + (noisy_ang_vel * sec)
+        avg_theta = (final_theta + init_theta) / 2.0
+        final_x = self.pose.position.x + (noisy_lin_vel * cos(avg_theta) * sec)
+        final_y = self.pose.position.y + (noisy_lin_vel * sin(avg_theta) * sec)
+
+        # Limit final_theta to range (-pi, pi)
+        final_theta = final_theta - (2 * pi) if final_theta > pi else final_theta
+        final_theta = final_theta + (2 * pi) if final_theta < -pi else final_theta
+
+        # Update current pose with new values
+        self.pose.position.x = final_x
+        self.pose.position.y = final_y
+        self.pose.orientation.x = final_theta
+
+
+    # Publish estimate of the robot's pose (if noise is set, won't exactly match true pose)
+    def send_pose_est(self):
+        self.pose_pub.publish(self.pose)
+
+
+if __name__ == "__main__":
+
+    # Create base image for viewing simulation
+    field_width = 5.0
+    field_length = 5.0
+    scale = 150
+    base_img = np.zeros((int(field_length * scale), int(field_width * scale), 3), np.uint8) + 244
+    p_color = (0, 150, 0)
+    r_color = (150, 0, 0)
+
+    # Initialize ROS node
+    rospy.init_node("path_tracking_simulator")
+
+    # Initial pose for robot
+    init_pose = Pose()
+    init_pose.position.x = 0.5
+    init_pose.position.y = 0.5
+    init_pose.orientation.x = pi / 4
+
+    # Create a simulated robot
+    robot = Robot("drive_cmd", "current_pose_estimate", init_pose)
+
+    # Set update rate for robots (how often path tracking node can receive new pose estimate)
+    loop_frequency = 100
+    update_rate = rospy.Rate(loop_frequency)
+
+    # Run simulation
+    while not rospy.is_shutdown():
+
+        # Move robot
+        robot.move_for_time(1.0 / loop_frequency)
+
+        # Send pose estimate
+        robot.send_pose_est()
+
+        # Copy the base image, draw the robot in it's current position
+        frame = np.copy(base_img)
+
+        # Get angle information from current pose
+        theta = robot.pose.orientation.x
+
+        # Draw arrow representing robot pose
+        arrow_x1 = robot.pose.position.x
+        arrow_y1 = field_length - robot.pose.position.y   # subtract so increased y = up in image
+        arrow_x2 = arrow_x1 + (cos(theta) * .15)
+        arrow_y2 = arrow_y1 - (sin(theta) * .15)
+        cv2.arrowedLine(frame, (int(arrow_x1 * scale), int(arrow_y1 * scale)),
+                        (int(arrow_x2 * scale), int(arrow_y2 * scale)), p_color, 2, tipLength=0.5)
+        cv2.circle(frame, (int(arrow_x1 * scale), int(arrow_y1 * scale)), 5, r_color)
+
+        # DRAW TEST POINT
+        target_point = [1.7071067811865475, 0.7071067811865475]
+        cv2.circle(frame, (int(target_point[0] * scale), int((field_length - target_point[1]) * scale)), 5, (0, 0, 255))
+
+        # Display image
+        cv2.imshow("Path tracking simulation", frame)
+        cv2.waitKey(1)
+
+        # Sleep to maintain loop rate
+        update_rate.sleep()
+
+