71 lines
2.2 KiB
C++
71 lines
2.2 KiB
C++
#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;
|
|
}
|
|
|