cleaning in prog
This commit is contained in:
parent
84013bf19a
commit
2af772c095
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 97d4093656a3ec9cfe8645457e00e55ccbdd8469
|
||||
|
|
@ -1,46 +0,0 @@
|
|||
#include <ros/ros.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <std_msgs/Float32MultiArray.h>
|
||||
#include <string>
|
||||
|
||||
// Declare global variables to store Axis values
|
||||
float axis0_value = 0.0;
|
||||
float axis1_value = 0.0;
|
||||
|
||||
void serial_data_callback(const std_msgs::String::ConstPtr& msg) {
|
||||
// Parse received string message
|
||||
std::string data_str = msg->data;
|
||||
if (data_str.find("Axis0") != std::string::npos) {
|
||||
// Extract value for Axis0
|
||||
axis0_value = std::stof(data_str.substr(data_str.find(":") + 1));
|
||||
} else if (data_str.find("Axis1") != std::string::npos) {
|
||||
// Extract value for Axis1
|
||||
axis1_value = std::stof(data_str.substr(data_str.find(":") + 1));
|
||||
}
|
||||
|
||||
// If both Axis0 and Axis1 values are available, publish them together
|
||||
if (axis0_value != 0.0 && axis1_value != 0.0) {
|
||||
std_msgs::Float32MultiArray float_array_msg;
|
||||
float_array_msg.data.push_back(axis0_value);
|
||||
float_array_msg.data.push_back(axis1_value);
|
||||
axis_values_pub.publish(float_array_msg);
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Initialize ROS node
|
||||
ros::init(argc, argv, "serial_data_parser");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// Subscribe to serial data topic
|
||||
ros::Subscriber sub = nh.subscribe("serial_data", 10, serial_data_callback);
|
||||
|
||||
// Initialize publisher for Axis values
|
||||
ros::Publisher axis_values_pub = nh.advertise<std_msgs::Float32MultiArray>("axis_values", 10);
|
||||
|
||||
// Spin ROS node
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
@ -1,70 +0,0 @@
|
|||
#include <ros/ros.h>
|
||||
#include <std_msgs/Float32MultiArray.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
// Constants
|
||||
const float DEAD_ZONE = 5.0; // Dead zone in percentage (0 to 100)
|
||||
const float MAX_SPEED = 1.0; // Maximum speed of the plate
|
||||
|
||||
// Global variables
|
||||
float y_position = 0.0; // Initial y position
|
||||
float prev_time = 0.0; // Previous time
|
||||
float direction = 0.0; // Direction of movement
|
||||
|
||||
void joystick_axis_callback(const std_msgs::Float32MultiArray::ConstPtr& msg) {
|
||||
// Extract joystick x and y positions
|
||||
float x_joystick = msg->data[0];
|
||||
float y_joystick = msg->data[1];
|
||||
|
||||
// Determine direction based on joystick position
|
||||
if (std::abs(x_joystick) < DEAD_ZONE) {
|
||||
direction = 0.0; // Joystick in dead zone, no movement
|
||||
} else {
|
||||
direction = (x_joystick > 0) ? 1.0 : -1.0; // Joystick outside dead zone, set direction accordingly
|
||||
}
|
||||
|
||||
// Calculate speed based on joystick position
|
||||
float speed = MAX_SPEED * std::abs(x_joystick) / 100.0;
|
||||
|
||||
// Calculate time elapsed since last callback
|
||||
float current_time = ros::Time::now().toSec();
|
||||
float time_elapsed = current_time - prev_time;
|
||||
|
||||
// Calculate change in y position
|
||||
y_position = y_position + time_elapsed * speed * direction;
|
||||
|
||||
// Store current time for next iteration
|
||||
prev_time = current_time;
|
||||
|
||||
// Publish robot position in a point format
|
||||
geometry_msgs::Point position_cmd;
|
||||
position_cmd.x = 65.0; // Fixed x-coordinate
|
||||
position_cmd.y = y_position;
|
||||
position_cmd.z = 0.0; // Assuming z-coordinate is not used
|
||||
|
||||
// Publish position command
|
||||
ros::NodeHandle nh;
|
||||
ros::Publisher position_cmd_pub = nh.advertise<geometry_msgs::Point>("/position_cmd", 10);
|
||||
position_cmd_pub.publish(position_cmd);
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
// Initialize ROS node
|
||||
ros::init(argc, argv, "joystick_to_robot_converter");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// Subscribe to joystick axis values topic
|
||||
ros::Subscriber joystick_sub = nh.subscribe("joystick_axis_values", 10, joystick_axis_callback);
|
||||
|
||||
// Initialize y position
|
||||
y_position = 0.0;
|
||||
|
||||
// Initialize previous time
|
||||
prev_time = ros::Time::now().toSec();
|
||||
|
||||
// Spin ROS node
|
||||
ros::spin();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue