Tạo node I3S_receiver

Sử dụng code từ beginner _tutorial, thay đổi receiver

#include <ros/ros.h>

#include <sensor_msgs/Joy.h>

#include <geometry_msgs/Twist.h>

#include <geometry_msgs/TwistStamped.h>

#include <std_msgs/Float64.h>

class JoyROV

{

public:

  JoyROV\(\);

  void joyCallback\(const sensor\_msgs::Joy::ConstPtr &joy\);



private:

  ros::NodeHandle nh;        



  ros::Subscriber joySub;



  ros::Publisher angVelPub;

  ros::Publisher throttlePub;

};

JoyROV::JoyROV(){

//nh.setParam\("/mavros/setpoint\_attitude/reverse\_throttle", true\);

joySub = nh.subscribe\("/joy", 10, &JoyROV::joyCallback, this\);



angVelPub = nh.advertise<geometry\_msgs::TwistStamped>\("/mavros/setpoint\_attitude/cmd_vel",10);

throttlePub = nh.advertise<std\_msgs::Float64>\("/mavros/setpoint\_attitude/att_throttle",10);

//reserve\_throttlePub = nh.advertise<bool>\("/mavros/setpoint\_attitude/reverse\_throttle",10\);  

    //param::set\("/mavros/setpoint\_attitude/reverse\_throttle", true\);

// void ros::param::set("/mavros/setpoint_attitude/reverse_throttle", true);

/*

//How to set parameter setpoit\_attitude/reserve\_throttle\(true\) for allowing to send -1.0 throttle???

bool reserve\_throttle\_ok = true;            

nh.setParam\("/mavros/setpoint\_attitude/reserve\_throttle", reserve\_throttle\_ok\);

/\*if \(nh.setParam\("/mavros/setpoint\_attitude/reserve\_throttle", reserve\_throttle\_ok\)\)

{

    ROS\_INFO\("Set sucessful param"\);

};\*/

}

void JoyROV::joyCallback(const sensor_msgs::Joy::ConstPtr &joy){

//ROS\_INFO\("I received: \[%f\]", joy->axes\[0\]\);    



geometry\_msgs::TwistStamped TwistStampedMsg;

TwistStampedMsg.twist.angular.x = joy->axes\[2\];// ROS\_INFO\("roll ang.vel received: \[%f\]" , TwistStampedMsg.twist.angular.x\);

TwistStampedMsg.twist.angular.y = joy->axes\[3\];// ROS\_INFO\("pitch ang.vel received: \[%f\]", TwistStampedMsg.twist.angular.y\);

TwistStampedMsg.twist.angular.z = joy->axes\[0\]; 

angVelPub.publish\(TwistStampedMsg\);



std\_msgs::Float64 throttleMsg;

throttleMsg.data = joy->axes\[1\];// ROS\_INFO\("throttle received: \[%f\]", throttleMsg.data\);

throttlePub.publish\(throttleMsg\);        

ROS\_INFO\("Debug: % 1.6f % 1.6f % 1.6f % 1.6f ", TwistStampedMsg.twist.angular.x, TwistStampedMsg.twist.angular.y, TwistStampedMsg.twist.angular.z, throttleMsg.data);

}

int main(int argc, char** argv){

ros::init\(argc, argv, "I3S\_receiver"\);

JoyROV I3S\_receiver;



ros::spin\(\);

return 0;

}