make forklift folder

catkin_ws/src/asclinic_pkg/src/forklift/fcontroller1.cpp

+#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;
+}

catkin_ws/src/asclinic_pkg/src/forklift/odometry.cpp

+#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;
+}
+

catkin_ws/src/asclinic_pkg/src/forklift/recieve_lidar.cpp

+#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;
+}