diff --git a/catkin_ws/src/asclinic_pkg/CMakeLists.txt b/catkin_ws/src/asclinic_pkg/CMakeLists.txt
index fdac4ef559db578507c0cc41a7ce556f4e62cacf..e682b6fb1796b0663ad67081fb8bb5224d046405 100644
--- a/catkin_ws/src/asclinic_pkg/CMakeLists.txt
+++ b/catkin_ws/src/asclinic_pkg/CMakeLists.txt
@@ -13,12 +13,12 @@ find_package(catkin REQUIRED COMPONENTS
rospy
std_msgs
geometry_msgs
+ tf
sensor_msgs
cv_bridge
genmsg
roslib
nav_msgs
- geometry_msgs
)
## System dependencies are found with CMake's conventions
@@ -220,6 +220,7 @@ add_executable(Kalman src/ascr/Kalman.cpp)
add_executable(fcontroller1 src/ascr/fcontroller1.cpp)
add_executable(final_controller src/ascr/final_controller.cpp)
add_executable(control_node src/forklift/control_node.cpp)
+add_executable(odometry_new src/forklift/odometry_new.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
@@ -255,6 +256,7 @@ add_dependencies(Kalman asclinic_pkg_generate_mes
add_dependencies(fcontroller1 asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(final_controller asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(control_node asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(odometry_new asclinic_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -287,6 +289,7 @@ target_link_libraries(Kalman ${catkin_LIBRARIES}
target_link_libraries(fcontroller1 ${catkin_LIBRARIES})
target_link_libraries(final_controller ${catkin_LIBRARIES})
target_link_libraries(control_node ${catkin_LIBRARIES})
+target_link_libraries(odometry_new ${catkin_LIBRARIES})
diff --git a/catkin_ws/src/asclinic_pkg/launch/forklift.launch b/catkin_ws/src/asclinic_pkg/launch/forklift.launch
new file mode 100644
index 0000000000000000000000000000000000000000..a3e61bc031901881b53167c94b13dd7713d41bcd
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/launch/forklift.launch
@@ -0,0 +1,102 @@
+<launch>
+
+ <!-- START A GROUP WITH A NAMESPACE -->
+ <group ns="asc">
+
+ <!-- LAUNCH A "I2C for Motors" NODE -->
+ <node
+ pkg = "asclinic_pkg"
+ name = "i2c_for_motors"
+ output = "screen"
+ type = "i2c_for_motors"
+ >
+ <param
+ name = "motor_driver_verbosity"
+ value = "1"
+ />
+ <param
+ name = "motor_driver_current_limit_in_milliamps"
+ value = "5000"
+ />
+ <param
+ name = "motor_driver_max_duty_cycle_limit_in_percent"
+ value = "100.0"
+ />
+ <param
+ name = "motor_driver_max_accel_limit_in_percent_per_millisecond"
+ value = "0.04"
+ />
+ <param
+ name = "motor_driver_max_decel_limit_in_percent_per_millisecond"
+ value = "0.16"
+ />
+ <param
+ name = "motor_driver_left_side_multiplier"
+ value = "1"
+ />
+ <param
+ name = "motor_driver_right_side_multiplier"
+ value = "1"
+ />
+ </node>
+
+ <node
+ pkg = "asclinic_pkg"
+ name = "encoder_read_multi_threaded"
+ output = "screen"
+ type = "encoder_read_multi_threaded"
+ >
+ <param
+ name = "encoder_read_verbosity"
+ value = "1"
+ />
+ <param
+ name = "gpio_chip_number_for_encoder_lines"
+ value = "1"
+ />
+ <param
+ name = "should_monitor_left_side_encoder_channel_a"
+ value = "true"
+ />
+ <param
+ name = "should_monitor_left_side_encoder_channel_b"
+ value = "false"
+ />
+ <param
+ name = "should_monitor_right_side_encoder_channel_a"
+ value = "true"
+ />
+ <param
+ name = "should_monitor_right_side_encoder_channel_b"
+ value = "false"
+ />
+ <param
+ name = "line_number_for_left_side_encoder_channel_a"
+ value = "105"
+ />
+ <param
+ name = "line_number_for_left_side_encoder_channel_b"
+ value = "106"
+ />
+ <param
+ name = "line_number_for_right_side_encoder_channel_a"
+ value = "84"
+ />
+ <param
+ name = "line_number_for_right_side_encoder_channel_b"
+ value = "130"
+ />
+ <!-- NOTE: options for event monitoring are: "rising", "falling", "both" -->
+ <param
+ name = "encoder_events_to_monitor"
+ value = "rising"
+ />
+ <param
+ name = "delta_t_for_publishing_encoder_counts"
+ value = "0.1"
+ />
+ </node>
+
+ </group>
+
+</launch>
\ No newline at end of file
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp
index a82925d68bcec79c62eb268407c7603b6c31a3b1..8732ade64fa59fb1ebaeacc1f423b2710e18f6eb 100644
--- a/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/control_node.cpp
@@ -30,7 +30,7 @@ public:
};
-PIDController pid(2, 0, 5);
+PIDController pid(3, 0, 5);
PIDController pid1(40, 0, 0);
PIDController pid_v(1.5, 0, 0);
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
deleted file mode 100644
index 1d44517cef39a8cc21d3d519043c2bb6c6f2b4ab..0000000000000000000000000000000000000000
--- a/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
+++ /dev/null
@@ -1,80 +0,0 @@
-#include "ros/ros.h"
-#include <ros/package.h>
-#include <cmath>
-#include "asclinic_pkg/LeftRightInt32.h"
-#include "asclinic_pkg/position.h"
-#include "asclinic_pkg/vw.h"
-// Robot's position and orientation variables
-float x = 0.0, y = 0.0, phi = 0.0;
-ros::Time time_previous, time_now; // Timing variables
-ros::Duration elapsed_time; // Duration to calculate elapsed time
-double time_float; // Floating-point representation of elapsed time
-
-// Physical and error parameters
-constexpr float WHEEL_RADIUS_LEFT = 0.072; // Left wheel radius in meters
-constexpr float WHEEL_RADIUS_RIGHT = 0.072; // Right wheel radius in meters
-constexpr float HALF_WHEEL_BASE = 0.125; // Half of the wheelbase in meters
-
-ros::Subscriber m_subscriber;
-ros::Publisher m_publisher;
-ros::Publisher m_angular_velocity_publisher; // Publisher for angular velocity as a single float
-
-void answer(const asclinic_pkg::LeftRightInt32& msg) {
- // Update times for elapsed time calculation
- time_previous = time_now;
- time_now = ros::Time::now();
- elapsed_time = time_now - time_previous;
- time_float = elapsed_time.toSec();
- ROS_INFO_STREAM("the seperation in second is "<<time_float);
-
- // Calculate angular displacements for left and right wheels
- float deltaThetaL = 0.000404 * msg.left / WHEEL_RADIUS_LEFT; // Calculate rotation angle for left wheel
- float deltaThetaR = 0.000404 * msg.right / WHEEL_RADIUS_RIGHT; // Calculate rotation angle for right wheel
-
- // Calculate the linear displacement and change in orientation
- float dS = (WHEEL_RADIUS_LEFT * deltaThetaL + WHEEL_RADIUS_RIGHT * deltaThetaR) / 2;
- float dPhi = (WHEEL_RADIUS_RIGHT * deltaThetaR - WHEEL_RADIUS_LEFT * deltaThetaL) / (2 * HALF_WHEEL_BASE);
-
- // Update position and orientation based on movements
- x += dS * cos(phi + 0.5 * dPhi);
- y += dS * sin(phi + 0.5 * dPhi);
- phi += dPhi;
-
- // Calculate angular velocity
- float angular_velocity = dPhi / time_float;
-
- // Log current position and angular velocity
- ROS_INFO_STREAM("Current Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity);
-
- // Publish position
- asclinic_pkg::position position_msg;
- position_msg.x = x;
- position_msg.y = y;
- position_msg.phi = phi;
- m_publisher.publish(position_msg);
-
- // Publish angular velocity
- asclinic_pkg::vw angular_velocity_msg;
- angular_velocity_msg.w = angular_velocity;
- m_angular_velocity_publisher.publish(angular_velocity_msg);
-}
-
-int main(int argc, char **argv) {
- ros::init(argc, argv, "receive_encoder");
-
- std::string ns_for_group = ros::this_node::getNamespace();
- ros::NodeHandle nh(ns_for_group);
-
- // Initialize time_now to current time
- time_now = ros::Time::now();
-
- m_subscriber = nh.subscribe("asc/encoder_counts", 1, answer);
- m_publisher = nh.advertise<asclinic_pkg::position>("current_position", 1);
- m_angular_velocity_publisher = nh.advertise<asclinic_pkg::vw>("angular_velocity_topic", 1);
-
- ROS_INFO_STREAM("Node started");
- ros::spin();
-
- return 0;
-}
-
diff --git a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
similarity index 53%
rename from catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
rename to catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
index d0ec56934546fe3dee594f670e5597dac0afb0ee..e6952a51c0b691d5721ae4938b9ad0958bac58b3 100644
--- a/catkin_ws/src/asclinic_pkg/src/ascr/odometry _new.cpp
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/odometry_new.cpp
@@ -2,33 +2,40 @@
#include <ros/package.h>
#include <cmath>
#include "asclinic_pkg/LeftRightInt32.h"
-#include "asclinic_pkg/position.h"
-#include "asclinic_pkg/vw.h"
-// Robot's position and orientation variables
-float x = 0.0, y = 0.0, phi = 0.0;
+#include "asclinic_pkg/LeftRightFloat32.h"
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
+#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/Twist.h>
+#include <tf/transform_datatypes.h> // For tf::createQuaternionMsgFromYaw
+
ros::Time time_previous, time_now; // Timing variables
ros::Duration elapsed_time; // Duration to calculate elapsed time
+
+// Robot's position and orientation variables
+float x = 0.0, y = 0.0, phi = 0.0;
double time_float; // Floating-point representation of elapsed time
+
// Physical and error parameters
constexpr float WHEEL_RADIUS_LEFT = 0.072; // Left wheel radius in meters
constexpr float WHEEL_RADIUS_RIGHT = 0.072; // Right wheel radius in meters
constexpr float HALF_WHEEL_BASE = 0.125; // Half of the wheelbase in meters
constexpr float k_l = 0.00023; // Uncertainty in the left wheel
constexpr float k_r = 0.0001; // Uncertainty in the right wheel
-int authority =0;
+int left_p = 1, right_p = 1;
+
+using Eigen::MatrixXd;
+
// Covariance matrix for pose
MatrixXd pose_covariance(3, 3); // Initialize a 3x3 matrix for pose covariance
-MatrixXd z_cov(3, 3);
ros::Subscriber m_subscriber;
-ros::Subscriber image_subscriber;
ros::Subscriber pwm_subscriber;
ros::Subscriber kalman_subscriber;
ros::Publisher m_publisher;
-ros::Publisher m_angular_velocity_publisher; // Publisher for angular velocity as a single float
ros::Publisher m_odometry_publisher; // Publisher for odometry
float normalize_angle(float angle) {
@@ -39,7 +46,20 @@ float normalize_angle(float angle) {
return 2 * M_PI - value;
}
}
-
+void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
+ if(msg.left<0){
+ left_p = -1;
+ }
+ else{
+ left_p = 1;
+ }
+ if(msg.right<0){
+ right_p = -1;
+ }
+ else{
+ right_p = 1;
+ }
+}
void answer(const asclinic_pkg::LeftRightInt32& msg) {
// Update times for elapsed time calculation
@@ -50,8 +70,8 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
ROS_INFO_STREAM("the seperation in second is "<<time_float);
// Calculate angular displacements for left and right wheels
- float deltaThetaL = 0.000404 * msg.left / WHEEL_RADIUS_LEFT; // Calculate rotation angle for left wheel
- float deltaThetaR = 0.000404 * msg.right / WHEEL_RADIUS_RIGHT; // Calculate rotation angle for right wheel
+ float deltaThetaL = 0.000404 * msg.left*left_p / WHEEL_RADIUS_LEFT; // Calculate rotation angle for left wheel
+ float deltaThetaR = 0.000404 * msg.right*right_p / WHEEL_RADIUS_RIGHT; // Calculate rotation angle for right wheel
// Calculate the linear displacement and change in orientation
float dS = (WHEEL_RADIUS_LEFT * deltaThetaL + WHEEL_RADIUS_RIGHT * deltaThetaR) / 2;
@@ -89,17 +109,7 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
// Log current position and angular velocity
ROS_INFO_STREAM("Current Position - X:" << x << ", Y:" << y << ", Phi:" << phi << ", Angular Velocity:" << angular_velocity);
- // Publish position
- asclinic_pkg::position position_msg;
- position_msg.x = x;
- position_msg.y = y;
- position_msg.phi = phi;
- m_publisher.publish(position_msg);
-
- // Publish angular velocity
- asclinic_pkg::vw angular_velocity_msg;
- angular_velocity_msg.w = angular_velocity;
- m_angular_velocity_publisher.publish(angular_velocity_msg);
+
// Publish odometry
nav_msgs::Odometry odom;
@@ -114,17 +124,21 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(phi);
// Set the velocity
- odom.twist.twist.linear.x = dS / time_float;
- odom.twist.twist.linear.y = 0.0;
+ odom.twist.twist.linear.x = dS * cos(phi + 0.5 * dPhi) / time_float;
+ odom.twist.twist.linear.y = dS * sin(phi + 0.5 * dPhi) / time_float;
odom.twist.twist.angular.z = angular_velocity;
// Covariance Matrix initialization
// Pose Covariance
- for (int i = 0; i < 3; ++i) {
- for (int j = 0; j < 3; ++j) {
- odom.pose.covariance[i * 6 + j] = pose_covariance(i, j);
- }
- }
+ odom.pose.covariance[0] = pose_covariance(0, 0); // x-x
+ odom.pose.covariance[1] = pose_covariance(0, 1); // x-y
+ odom.pose.covariance[5] = pose_covariance(0, 2); // x-yaw
+ odom.pose.covariance[6] = pose_covariance(1, 0); // y-x
+ odom.pose.covariance[7] = pose_covariance(1, 1); // y-y
+ odom.pose.covariance[11] = pose_covariance(1, 2); // y-yaw
+ odom.pose.covariance[30] = pose_covariance(2, 0); // yaw-x
+ odom.pose.covariance[31] = pose_covariance(2, 1); // yaw-y
+ odom.pose.covariance[35] = pose_covariance(2, 2); // yaw-yaw
// Twist covariance
for (int i = 0; i < 36; ++i) {
@@ -133,108 +147,11 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) {
m_odometry_publisher.publish(odom);
}
-void ImageAnswer(const asclinic_pkg::position& msg) {
- constrain_time = ros::Time::now();
- constrain_duration = constrain_time - begin_time;
- float temp_value = constrain_duration.toSec();
- if (temp_value < 50) {
- return;
- }
- // Check if elapsed time is less than 0.5 seconds
- if (authority ==0) {
- return;
- }
-
- // Update the last time to the current time
-
- MatrixXd z_position(3, 1);
- z_position << msg.x,
- msg.y,
- msg.phi;
-
- MatrixXd K = pose_covariance * (pose_covariance + z_cov).inverse();
- MatrixXd odo_position(3, 1);
- odo_position << x,
- y,
- phi;
-
- //ROS_INFO_STREAM("odo_position: " << odo_position);
- //ROS_INFO_STREAM("z_position: " << z_position);
- //ROS_INFO_STREAM("K: " << K);
-
- MatrixXd new_pose = odo_position + K * (z_position - odo_position);
-
- //ROS_INFO_STREAM("new_pose: " << new_pose);
-
- if (std::isnan(new_pose(0)) || std::isnan(new_pose(1)) || std::isnan(new_pose(2))||new_pose(0)<-2 ||new_pose(1)<-4||std::abs(odo_position(0)-new_pose(0))>1.5||std::abs(odo_position(1)-new_pose(1)>1.5)) {
- x = odo_position(0);
- y = odo_position(1);
- phi = odo_position(2);
- ROS_WARN("Fail: new_pose contains NaN values");
- } else {
- x = new_pose(0);
- y = new_pose(1);
- phi = new_pose(2);
- pose_covariance = pose_covariance - K * (pose_covariance + z_cov) * K.transpose();
- ROS_INFO_STREAM("Updated pose: X:" << x << ", Y:" << y << ", Phi:" << phi);
- }
-
- phi = normalize_angle(phi);
- //ROS_INFO_STREAM("Normalized Phi: " << phi);
-
- ROS_INFO("Image updated");
- authority = 0;
-
- std_msgs::UInt32 Umsg;
- Umsg.data = 1;
- kalman_output.publish(Umsg);
-}
-
-
-void pwmAnswer(const asclinic_pkg::LeftRightFloat32& msg){
- if(msg.left<0){
- left_p = -1;
- }
- else{
- left_p = 1;
- }
- if(msg.right<0){
- right_p = -1;
- }
- else{
- right_p = 1;
- }
-}
-
-void update_authority(const std_msgs::UInt32& msg) {
- authority = 1;
- ros::Time last_time = ros::Time::now();
- ros::Time current_time = ros::Time::now();
- ros::Duration lastest_duration;
- float duration_authority;
- ROS_INFO_STREAM("Authority updated to: " << authority);
- while(authority){
- current_time = ros::Time::now();
- lastest_duration = current_time - last_time;
- duration_authority = lastest_duration.toSec();
- if(duration_authority > 2){
- authority = 0;
- std_msgs::UInt32 Umsg;
- Umsg.data = 1;
- kalman_output.publish(Umsg);
- ROS_INFO_STREAM("go with no update");
- }
-
- ros::spinOnce();
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
-}
int main(int argc, char **argv) {
- ros::init(argc, argv, "Kalman");
+ ros::init(argc, argv, "odometry_new");
std::string ns_for_group = ros::this_node::getNamespace();
ros::NodeHandle nh(ns_for_group);
- begin_time = ros::Time::now();
// Initialize time_now to current time
time_now = ros::Time::now();
@@ -242,19 +159,11 @@ int main(int argc, char **argv) {
1, 1, 1,
1, 1, 1;
- z_cov << 10, 10, 10,
- 10, 10, 10,
- 10, 10, 10;
+
m_subscriber = nh.subscribe("/asc/encoder_counts", 1, answer);
- image_subscriber = nh.subscribe("/image_estimation", 1, ImageAnswer);
pwm_subscriber = nh.subscribe("/asc/current_motor_duty_cycle", 1, pwmAnswer);
- kalman_subscriber = nh.subscribe("/kalman_demand", 1, update_authority);
-
- m_publisher = nh.advertise<asclinic_pkg::position>("/current_position", 10);
- m_angular_velocity_publisher = nh.advertise<asclinic_pkg::vw>("/angular_velocity_topic", 10);
- kalman_output = nh.advertise<std_msgs::UInt32>("/kalman_info", 1);
- m_odometry_publisher = nh.advertise<nav_msgs/Odometry>("/odom", 10);
+ m_odometry_publisher = nh.advertise<nav_msgs::Odometry>("/odom", 10);
ROS_INFO_STREAM("Node started");
ros::spin();
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/ppf.py b/catkin_ws/src/asclinic_pkg/src/forklift/ppf.py
new file mode 100755
index 0000000000000000000000000000000000000000..ee5ecfa979e10f2389ef5d4565010fa44ca72cb5
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/ppf.py
@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+
+import rospy
+
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Twist
+import matplotlib.pyplot as plt
+
+# Global variables to store position data
+x_data = []
+y_data = []
+
+# Obstacle coordinates
+ox = []
+oy = []
+
+# Define obstacles
+for i in range(-4, 6):
+ ox.append(-2)
+ oy.append(i)
+for i in range(-4, 6):
+ ox.append(8)
+ oy.append(i)
+for i in range(-2, 9):
+ ox.append(i)
+ oy.append(5)
+for i in range(-2, 9):
+ ox.append(i)
+ oy.append(-4)
+# Table obstacles
+for i in range(25, 36, 5): # Range from 2.5 to 3.4 (exclusive), multiplied by 10 for integer range
+ ox.append(i / 10) # Divide by 10 to get the actual floating-point value
+ oy.append(-3.25)
+for i in range(25, 36, 5):
+ ox.append(i / 10)
+ oy.append(-2.75)
+for i in range(-325, -275, 50): # Range from -3.25 to 2.74 (exclusive), multiplied by 100 for integer range
+ ox.append(2.5)
+ oy.append(i / 100) # Divide by 100 to get the actual floating-point value
+for i in range(-325, -275, 50):
+ ox.append(3.5)
+ oy.append(i / 100)
+
+# Additional obstacles
+ox1 =[1.973, 2.033, 3.982, 3.982, 5.953, 6.013, 7.956, 7.956, 7.986, 7.986, 0.0, 0.0, -1.99, -1.99, 0.0, 2.315, 3.0, 3.687, 5.045, 7.712, 5.983, 3.982, 2.003, 0.0, -1.99, 7.712]
+
+oy1 =[0.0, 0.0, 0.955, 1.015, 0.0, 0.0, 0.955, 1.015, -0.106, 2.0, 0.955, 1.015, 0.0, -3.0, -4.523, -3.1, -2.696, -3.1, -4.523, 4.0, 5.474, 5.474, 5.474, 5.474, 3.0, -2.0]
+ox.extend(ox1)
+oy.extend(oy1)
+
+def position_callback(msg):
+ global x_data, y_data
+ x_data.append(msg.pose.pose.position.x)
+ y_data.append(msg.pose.pose.position.y)
+
+def main():
+ rospy.init_node('ppf')
+ rospy.Subscriber('/odom', Odometry, position_callback)
+
+ plt.ion() # Enable interactive mode
+ fig, ax = plt.subplots()
+ rate = rospy.Rate(5) # Adjust the rate as needed
+
+ while not rospy.is_shutdown():
+ ax.clear()
+ ax.plot(ox, oy, 'ro', label='Obstacles') # Plot obstacles
+ ax.plot(x_data, y_data, 'b-', label='Path')
+
+ ax.set_xlabel('X Position')
+ ax.set_ylabel('Y Position')
+ ax.set_title('Robot Position Plot')
+ ax.grid(True)
+ ax.legend()
+ plt.pause(0.1)
+ rate.sleep()
+
+ plt.ioff()
+ plt.show()
+
+if __name__ == '__main__':
+ try:
+ main()
+ except rospy.ROSInterruptException:
+ pass
diff --git a/catkin_ws/src/teleop_twist_keyboard/CHANGELOG.rst b/catkin_ws/src/teleop_twist_keyboard/CHANGELOG.rst
new file mode 100644
index 0000000000000000000000000000000000000000..8d8d204ce41a19687ce7049dc4c5341633d18e23
--- /dev/null
+++ b/catkin_ws/src/teleop_twist_keyboard/CHANGELOG.rst
@@ -0,0 +1,40 @@
+1.0.0 (2020-06-26)
+------------------
+* Implement repeat rate and key timeout
+* Update readme about publishing to another topic.
+* Contributors: Austin, trainman419
+
+0.6.2 (2018-10-21)
+------------------
+* Replace tabs with spaces, fixes `#15 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/15>`_
+* Merge pull request `#13 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/13>`_ from asukiaaa/patch-3
+ Add rosrun command to specify values
+* Add rosrun command to specify values
+* Contributors: Asuki Kono, Austin, trainman419
+
+0.6.1 (2018-05-02)
+------------------
+* Merge pull request `#11 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/11>`_ from MatthijsBurgh/patch-1
+ Correct exception handling; Python3 print compatible
+* import print from future
+* Print python3 compatible
+* correct Exception handling
+* Merge pull request `#7 <https://github.com/ros-teleop/teleop_twist_keyboard/issues/7>`_ from lucasw/speed_params
+ set linear and turn speed via rosparams
+* Using tabs instead of spaces to match rest of file
+* set linear and turn speed via rosparams
+* Contributors: Austin, Lucas Walter, Matthijs van der Burgh
+
+0.6.0 (2016-03-21)
+------------------
+* Better instruction formatting
+* support holonomic base, send commmand with keyboard down
+* Fixing queue_size warning
+* Create README.md
+* Update the description string in package.xml
+* Contributors: Austin, Kei Okada, LiohAu, Mike Purvis, kk6axq, trainman419
+
+0.5.0 (2014-02-11)
+------------------
+* Initial import from brown_remotelab
+* Convert to catkin
diff --git a/catkin_ws/src/teleop_twist_keyboard/CMakeLists.txt b/catkin_ws/src/teleop_twist_keyboard/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..86d90cb1be78fd87efc158078d6b7265248b26dc
--- /dev/null
+++ b/catkin_ws/src/teleop_twist_keyboard/CMakeLists.txt
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(teleop_twist_keyboard)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+ teleop_twist_keyboard.py
+ DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
diff --git a/catkin_ws/src/teleop_twist_keyboard/README.md b/catkin_ws/src/teleop_twist_keyboard/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..000d050901274f62507ae495215d8e9d85277666
--- /dev/null
+++ b/catkin_ws/src/teleop_twist_keyboard/README.md
@@ -0,0 +1,74 @@
+# teleop_twist_keyboard
+Generic Keyboard Teleop for ROS
+
+# Launch
+Run.
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py
+```
+
+With custom values.
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _speed:=0.9 _turn:=0.8
+```
+
+Publishing to a different topic (in this case `my_cmd_vel`).
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=my_cmd_vel
+```
+
+# Usage
+```
+Reading from the keyboard and Publishing to Twist!
+---------------------------
+Moving around:
+ u i o
+ j k l
+ m , .
+
+For Holonomic mode (strafing), hold down the shift key:
+---------------------------
+ U I O
+ J K L
+ M < >
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
+
+q/z : increase/decrease max speeds by 10%
+w/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
+
+CTRL-C to quit
+```
+
+# Repeat Rate
+
+If your mobile base requires constant updates on the cmd\_vel topic, teleop\_twist\_keyboard can be configured to repeat the last command at a fixed interval, using the `repeat_rate` private parameter.
+
+For example, to repeat the last command at 10Hz:
+
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _repeat_rate:=10.0
+```
+
+It is _highly_ recommened that the repeat rate be used in conjunction with the key timeout, to prevent runaway robots.
+
+# Key Timeout
+
+Teleop\_twist\_keyboard can be configured to stop your robot if it does not receive any key presses in a configured time period, using the `key_timeout` private parameter.
+
+For example, to stop your robot if a keypress has not been received in 0.6 seconds:
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _key_timeout:=0.6
+```
+
+It is recommended that you set `key_timeout` higher than the initial key repeat delay on your system (This delay is 0.5 seconds by default on Ubuntu, but can be adjusted).
+
+# Twist with header
+Publishing a `TwistStamped` message instead of `Twist` can be enabled with the `stamped` private parameter. Additionally the `frame_id` of the `TwistStamped` message can be set with the `frame_id` private parameter.
+```
+rosrun teleop_twist_keyboard teleop_twist_keyboard.py _stamped:=True _frame_id:=base_link
+```
\ No newline at end of file
diff --git a/catkin_ws/src/teleop_twist_keyboard/package.xml b/catkin_ws/src/teleop_twist_keyboard/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..84add0221dc56865e5c80751a7f413d92c15979f
--- /dev/null
+++ b/catkin_ws/src/teleop_twist_keyboard/package.xml
@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package>
+ <name>teleop_twist_keyboard</name>
+ <version>1.0.0</version>
+ <description>Generic keyboard teleop for twist robots.</description>
+
+ <maintainer email="namniart@gmail.com">Austin Hendrix</maintainer>
+
+ <license>BSD</license>
+
+ <url type="website">http://wiki.ros.org/teleop_twist_keyboard</url>
+
+ <author>Graylin Trevor Jay</author>
+
+ <buildtool_depend>catkin</buildtool_depend>
+ <run_depend>geometry_msgs</run_depend>
+ <run_depend>rospy</run_depend>
+</package>
diff --git a/catkin_ws/src/teleop_twist_keyboard/teleop_twist_keyboard.py b/catkin_ws/src/teleop_twist_keyboard/teleop_twist_keyboard.py
new file mode 100755
index 0000000000000000000000000000000000000000..a85e1cc5c0c6bd10fa8779bf6ad6b4ed18c36f2a
--- /dev/null
+++ b/catkin_ws/src/teleop_twist_keyboard/teleop_twist_keyboard.py
@@ -0,0 +1,265 @@
+#!/usr/bin/env python
+
+from __future__ import print_function
+
+import threading
+
+import roslib; roslib.load_manifest('teleop_twist_keyboard')
+import rospy
+
+from geometry_msgs.msg import Twist
+from geometry_msgs.msg import TwistStamped
+
+import sys
+from select import select
+
+if sys.platform == 'win32':
+ import msvcrt
+else:
+ import termios
+ import tty
+
+
+TwistMsg = Twist
+
+msg = """
+Reading from the keyboard and Publishing to Twist!
+---------------------------
+Moving around:
+ u i o
+ j k l
+ m , .
+
+For Holonomic mode (strafing), hold down the shift key:
+---------------------------
+ U I O
+ J K L
+ M < >
+
+t : up (+z)
+b : down (-z)
+
+anything else : stop
+
+q/z : increase/decrease max speeds by 10%
+w/x : increase/decrease only linear speed by 10%
+e/c : increase/decrease only angular speed by 10%
+
+CTRL-C to quit
+"""
+
+moveBindings = {
+ 'i':(1,0,0,0),
+ 'o':(1,0,0,-1),
+ 'j':(0,0,0,1),
+ 'l':(0,0,0,-1),
+ 'u':(1,0,0,1),
+ ',':(-1,0,0,0),
+ '.':(-1,0,0,1),
+ 'm':(-1,0,0,-1),
+ 'O':(1,-1,0,0),
+ 'I':(1,0,0,0),
+ 'J':(0,1,0,0),
+ 'L':(0,-1,0,0),
+ 'U':(1,1,0,0),
+ '<':(-1,0,0,0),
+ '>':(-1,-1,0,0),
+ 'M':(-1,1,0,0),
+ 't':(0,0,1,0),
+ 'b':(0,0,-1,0),
+ }
+
+speedBindings={
+ 'q':(1.1,1.1),
+ 'z':(.9,.9),
+ 'w':(1.1,1),
+ 'x':(.9,1),
+ 'e':(1,1.1),
+ 'c':(1,.9),
+ }
+
+class PublishThread(threading.Thread):
+ def __init__(self, rate):
+ super(PublishThread, self).__init__()
+ self.publisher = rospy.Publisher('cmd_vel', TwistMsg, queue_size = 1)
+ self.x = 0.0
+ self.y = 0.0
+ self.z = 0.0
+ self.th = 0.0
+ self.speed = 0.0
+ self.turn = 0.0
+ self.condition = threading.Condition()
+ self.done = False
+
+ # Set timeout to None if rate is 0 (causes new_message to wait forever
+ # for new data to publish)
+ if rate != 0.0:
+ self.timeout = 1.0 / rate
+ else:
+ self.timeout = None
+
+ self.start()
+
+ def wait_for_subscribers(self):
+ i = 0
+ while not rospy.is_shutdown() and self.publisher.get_num_connections() == 0:
+ if i == 4:
+ print("Waiting for subscriber to connect to {}".format(self.publisher.name))
+ rospy.sleep(0.5)
+ i += 1
+ i = i % 5
+ if rospy.is_shutdown():
+ raise Exception("Got shutdown request before subscribers connected")
+
+ def update(self, x, y, z, th, speed, turn):
+ self.condition.acquire()
+ self.x = x
+ self.y = y
+ self.z = z
+ self.th = th
+ self.speed = speed
+ self.turn = turn
+ # Notify publish thread that we have a new message.
+ self.condition.notify()
+ self.condition.release()
+
+ def stop(self):
+ self.done = True
+ self.update(0, 0, 0, 0, 0, 0)
+ self.join()
+
+ def run(self):
+ twist_msg = TwistMsg()
+
+ if stamped:
+ twist = twist_msg.twist
+ twist_msg.header.stamp = rospy.Time.now()
+ twist_msg.header.frame_id = twist_frame
+ else:
+ twist = twist_msg
+ while not self.done:
+ if stamped:
+ twist_msg.header.stamp = rospy.Time.now()
+ self.condition.acquire()
+ # Wait for a new message or timeout.
+ self.condition.wait(self.timeout)
+
+ # Copy state into twist message.
+ twist.linear.x = self.x * self.speed
+ twist.linear.y = self.y * self.speed
+ twist.linear.z = self.z * self.speed
+ twist.angular.x = 0
+ twist.angular.y = 0
+ twist.angular.z = self.th * self.turn
+
+ self.condition.release()
+
+ # Publish.
+ self.publisher.publish(twist_msg)
+
+ # Publish stop message when thread exits.
+ twist.linear.x = 0
+ twist.linear.y = 0
+ twist.linear.z = 0
+ twist.angular.x = 0
+ twist.angular.y = 0
+ twist.angular.z = 0
+ self.publisher.publish(twist_msg)
+
+
+def getKey(settings, timeout):
+ if sys.platform == 'win32':
+ # getwch() returns a string on Windows
+ key = msvcrt.getwch()
+ else:
+ tty.setraw(sys.stdin.fileno())
+ # sys.stdin.read() returns a string on Linux
+ rlist, _, _ = select([sys.stdin], [], [], timeout)
+ if rlist:
+ key = sys.stdin.read(1)
+ else:
+ key = ''
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
+ return key
+
+def saveTerminalSettings():
+ if sys.platform == 'win32':
+ return None
+ return termios.tcgetattr(sys.stdin)
+
+def restoreTerminalSettings(old_settings):
+ if sys.platform == 'win32':
+ return
+ termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
+
+def vels(speed, turn):
+ return "currently:\tspeed %s\tturn %s " % (speed,turn)
+
+if __name__=="__main__":
+ settings = saveTerminalSettings()
+
+ rospy.init_node('teleop_twist_keyboard')
+
+ speed = rospy.get_param("~speed", 0.5)
+ turn = rospy.get_param("~turn", 1.0)
+ speed_limit = rospy.get_param("~speed_limit", 1000)
+ turn_limit = rospy.get_param("~turn_limit", 1000)
+ repeat = rospy.get_param("~repeat_rate", 0.0)
+ key_timeout = rospy.get_param("~key_timeout", 0.5)
+ stamped = rospy.get_param("~stamped", False)
+ twist_frame = rospy.get_param("~frame_id", '')
+ if stamped:
+ TwistMsg = TwistStamped
+
+ pub_thread = PublishThread(repeat)
+
+ x = 0
+ y = 0
+ z = 0
+ th = 0
+ status = 0
+
+ try:
+ pub_thread.wait_for_subscribers()
+ pub_thread.update(x, y, z, th, speed, turn)
+
+ print(msg)
+ print(vels(speed,turn))
+ while(1):
+ key = getKey(settings, key_timeout)
+ if key in moveBindings.keys():
+ x = moveBindings[key][0]
+ y = moveBindings[key][1]
+ z = moveBindings[key][2]
+ th = moveBindings[key][3]
+ elif key in speedBindings.keys():
+ speed = min(speed_limit, speed * speedBindings[key][0])
+ turn = min(turn_limit, turn * speedBindings[key][1])
+ if speed == speed_limit:
+ print("Linear speed limit reached!")
+ if turn == turn_limit:
+ print("Angular speed limit reached!")
+ print(vels(speed,turn))
+ if (status == 14):
+ print(msg)
+ status = (status + 1) % 15
+ else:
+ # Skip updating cmd_vel if key timeout and robot already
+ # stopped.
+ if key == '' and x == 0 and y == 0 and z == 0 and th == 0:
+ continue
+ x = 0
+ y = 0
+ z = 0
+ th = 0
+ if (key == '\x03'):
+ break
+
+ pub_thread.update(x, y, z, th, speed, turn)
+
+ except Exception as e:
+ print(e)
+
+ finally:
+ pub_thread.stop()
+ restoreTerminalSettings(settings)