From f4d06a623049084a48ad3a3fd4d6478ea3874fab Mon Sep 17 00:00:00 2001
From: Ethan <jiaao@student.unimelb.edu.au>
Date: Wed, 17 Jul 2024 13:05:06 +0800
Subject: [PATCH] make forklift folder
---
.../src/forklift/fcontroller1.cpp | 220 ++++++++++++++++++
.../asclinic_pkg/src/forklift/odometry.cpp | 80 +++++++
.../src/forklift/recieve_lidar.cpp | 40 ++++
3 files changed, 340 insertions(+)
create mode 100644 catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp
create mode 100644 catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
create mode 100644 catkin_ws/src/asclinic_pkg/src/forklift/recieve_lidar.cpp
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp
new file mode 100644
index 00000000..376db272
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp
@@ -0,0 +1,220 @@
+#define _USE_MATH_DEFINES
+#include "ros/ros.h"
+#include "asclinic_pkg/vw.h"
+#include <math.h>
+#include <fstream>
+#include <thread>
+#include <mutex>
+#include <chrono>
+#include "asclinic_pkg/LeftRightFloat32.h"
+#include "asclinic_pkg/position.h" // Assuming this is your custom message header
+#include "asclinic_pkg/rotation.h"
+#include <std_msgs/UInt32.h>
+
+class PIDController {
+private:
+ float Kp, Ki, Kd;
+ float prev_error;
+ float integral;
+
+public:
+ PIDController(float p, float i, float d) : Kp(p), Ki(i), Kd(d), prev_error(0), integral(0) {}
+
+ float compute(float error, float dt) {
+ float derivative = (error - prev_error) / dt;
+ integral += error * dt; // Proper integration over time
+ float output = Kp * error + Ki * integral + Kd * derivative;
+ prev_error = error;
+ return output;
+ }
+ void resetPrevError() {
+ prev_error = 0;
+ }
+};
+
+
+PIDController pid(2, 0, 5);
+PIDController pid1(40, 0, 0);
+PIDController pid_v(1.5, 0, 0);
+
+ros::Subscriber vw_sub;
+ros::Subscriber vw_r_sub;
+ros::Subscriber odometry_sub;
+ros::Subscriber position_sub;
+ros::Subscriber kalman_back;
+
+ros::Publisher output_pub;
+ros::Publisher state_pub;
+ros::Publisher kalman_request;
+
+std::mutex mtx;
+
+float current_phi;
+float ref_phi;
+float time_sep = 0.1;
+float ref_v = 0;
+float ref_w = 0;
+float w_now = 0;
+float v_now = 0;
+int state_value = 2; // 0 is forward, 1 is rotate , 2 is pending
+
+float initial_pwm = 25;
+//initial 26.5
+void straightline(const asclinic_pkg::vw& vw_msg) {
+
+
+ ref_v = vw_msg.v;
+ ref_w = vw_msg.w;
+
+
+ if(ref_v==0 && ref_w == 0){
+ state_value = 2;
+ ROS_INFO_STREAM("stopping");
+ asclinic_pkg::LeftRightFloat32 output_msg;
+ output_msg.left = 0;
+ output_msg.right = 0;
+ output_pub.publish(output_msg);
+ output_pub.publish(output_msg);
+
+
+ }
+ else{
+ state_value = 0;
+ }
+
+}
+
+void purerotation(const asclinic_pkg::rotation& rotation_msg) {
+ state_value = 1;
+ float flag = rotation_msg.direction;
+ ref_phi = rotation_msg.ref_phi;
+
+ float rotating_pwm = 15;
+ float right;
+ float left;
+ if (flag > 0) {
+ right = rotating_pwm;
+ left = -rotating_pwm;
+ } else {
+ right = -rotating_pwm;
+ left = rotating_pwm;
+ }
+
+ ROS_INFO_STREAM("The right rotation percentage is " << right);
+ asclinic_pkg::LeftRightFloat32 output_msg;
+ output_msg.left = left;
+ output_msg.right = right;
+ output_pub.publish(output_msg);
+ ROS_INFO_STREAM("First message of turning sent");
+
+
+
+ // Main rotation loop
+ bool condition = true;
+ while (condition) {
+ float current_phi_value;
+ {
+ std::lock_guard<std::mutex> lock(mtx);
+ current_phi_value = current_phi;
+ }
+
+ float error = std::abs(ref_phi - current_phi_value);
+ ROS_INFO_STREAM("Current phi: " << current_phi_value << ", Ref phi: " << ref_phi << ", Error: " << error);
+
+ if (error <= M_PI / 8) {
+ condition = false;
+ ROS_INFO("Reached desired rotation angle. Stopping rotation.");
+
+ // Publish PWM signals to stop rotation
+ output_msg.left = 0;
+ output_msg.right = 0;
+ output_pub.publish(output_msg);
+ }
+
+ ros::spinOnce();
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+
+ // Ensure motors are stopped
+ output_msg.left = 0;
+ output_msg.right = 0;
+ output_pub.publish(output_msg);
+ ROS_INFO_STREAM("Motors stopped after rotation");
+
+ std_msgs::UInt32 Umsg;
+ Umsg.data = 1;
+ kalman_request.publish(Umsg);
+
+
+
+}
+
+void kalman_change(const std_msgs::UInt32& Umsg){
+
+ // Publish state message
+ std_msgs::UInt32 state_msg;
+ state_msg.data = 1;
+ state_pub.publish(state_msg);
+ ROS_INFO_STREAM("State message published");
+}
+
+void updateO(const asclinic_pkg::vw& w_msg) {
+ w_now = w_msg.w;
+ v_now = w_msg.v;
+
+ if (state_value != 0){
+ return;
+ }
+
+ //compute the error
+ float error = ref_w - w_now;
+ float error_v = ref_v - v_now;
+ //calculate the output of control
+ float control_output = pid.compute(error, time_sep);
+ float control_output_v = pid_v.compute(error_v, time_sep);
+
+ float left = initial_pwm - control_output / 2;
+ float right = initial_pwm + control_output / 2;
+ if (right > 95) {
+ right = 90;
+ } else if (right < -95) {
+ right = -90;
+ }
+ if (left > 95) {
+ left = 90;
+ } else if (left < -95) {
+ left = -90;
+ }
+ asclinic_pkg::LeftRightFloat32 output_msg;
+ output_msg.left = left;
+ output_msg.right = right;
+ output_pub.publish(output_msg);
+
+
+
+}
+void updatep(const asclinic_pkg::position& msg){
+ std::lock_guard<std::mutex> lock(mtx);
+ current_phi = msg.phi;
+
+}
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "fcontroller1");
+ std::string ns_for_group = ros::this_node::getNamespace();
+ ros::NodeHandle nh(ns_for_group);
+ ROS_INFO_STREAM("node started");
+
+ vw_sub = nh.subscribe("/vw_information", 1, straightline);
+ vw_r_sub = nh.subscribe("/vw_r_information", 3, purerotation);
+ odometry_sub = nh.subscribe("/angular_velocity_topic", 1, updateO);
+ position_sub = nh.subscribe("/current_position", 1, updatep);
+ kalman_back = nh.subscribe("/kalman_info", 1, kalman_change);
+
+ output_pub = nh.advertise<asclinic_pkg::LeftRightFloat32>("/asc/set_motor_duty_cycle", 1, false);
+ state_pub = nh.advertise<std_msgs::UInt32>("/controller1_state", 3, false);
+ kalman_request = nh.advertise<std_msgs::UInt32>("/kalman_demand", 1, false);
+ ros::spin();
+
+ return 0;
+}
diff --git a/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
new file mode 100644
index 00000000..1d44517c
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp
@@ -0,0 +1,80 @@
+#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/forklift/recieve_lidar.cpp b/catkin_ws/src/asclinic_pkg/src/forklift/recieve_lidar.cpp
new file mode 100644
index 00000000..47e12cd2
--- /dev/null
+++ b/catkin_ws/src/asclinic_pkg/src/forklift/recieve_lidar.cpp
@@ -0,0 +1,40 @@
+#include "ros/ros.h"
+#include "sensor_msgs/LaserScan.h"
+#include <fstream>
+
+std::ofstream outputFile;
+ros::Subscriber m_subscriber;
+
+void Answer(const sensor_msgs::LaserScan& msg) {
+ ROS_INFO_STREAM("Message received with angle_min = " << msg.angle_min << " [rad], angle_max = " << msg.angle_max << " [rad], range_min = " << msg.range_min << " [m], range_max = " << msg.range_max << " [m]");
+
+ if (outputFile.is_open()) {
+ for (size_t i = 0; i < msg.ranges.size(); ++i) {
+ outputFile << msg.ranges[i] << ",";
+ }
+ outputFile << std::endl;
+ } else {
+ ROS_ERROR_STREAM("Unable to write to file. File is not open.");
+ }
+}
+
+int main(int argc, char* argv[]) {
+ ros::init(argc, argv, "receive_lidar");
+ ros::NodeHandle nh;
+
+ std::string outputFilePath = "/home/asc/lidar_data.csv";
+ outputFile.open(outputFilePath, std::ios::out | std::ios::trunc);
+ if (!outputFile.is_open()) {
+ ROS_ERROR_STREAM("Unable to create/open file " << outputFilePath);
+ return 1;
+ }
+
+ outputFile << "ranges" << std::endl;
+
+ ROS_INFO_STREAM("Node started");
+ m_subscriber = nh.subscribe("asc/scan", 1, Answer);
+ ros::spin();
+
+ outputFile.close();
+ return 0;
+}
--
GitLab