ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
Thanks to @Delb and @gvdhoom. Here is my working solution. I didn't implement the spin thread since it would consume a lot of cpu resource.
#include <ros/ros.h>
#include <std_msgs/Float32.h>
#include <sensor_msgs/Joy.h>
#include <std_msgs/Bool.h>
bool flag;
class Override
{
public:
Override();
void joyCallback(const sensor_msgs::Joy::ConstPtr& joy);
void Publisher();
std_msgs::Bool override_status;
private:
ros::NodeHandle nh_;
ros::Publisher override_pub;
ros::Subscriber joy_sub_;
bool currentReading;
bool lastReading;
std_msgs::Float32 button_status;
};
Override::Override()
{
override_pub = nh_.advertise<std_msgs::Bool>("override_status", 1);
joy_sub_ = nh_.subscribe<sensor_msgs::Joy>("joy", 10, &Override::joyCallback, this);
}
void Override::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
{
button_status.data=joy->buttons[4];
if (button_status.data == 0.0){
currentReading = false;
}else currentReading = true;
if (currentReading && !lastReading) {
flag=!flag;
if (flag) {
override_status.data = true;
}
else {
override_status.data = false;
}
}
lastReading = currentReading;
}
void Override::Publisher()
{
override_pub.publish(override_status);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "override_node");
ROS_INFO("Node Started...");
Override override;
ros::Rate r(10);
while (ros::ok()){
override.Publisher();
ros::spinOnce();
}
}