Hello everyone, I am currently try to control a Turtlebot3 rover by setting the right values of the PWM outputs on the pixhawk autopilot using MAVROS and MAVLink protocol from my laptop.
My goal is to make the rover move by choosing the values for the PWM signals on pins 2 and 3 (steering and throttle) that will then be traduced by the OPENCR board of the Turtlebot that controls the motors.
The Pixhawk is rightly connected and set in OFFBOARD mode (i used QGroundControl to upload the Rover firmware on the autopilot).
I wrote a script (control_actuator.cpp) that uses set_actuator_control messages and sets the values for the pins (the script is available at the end of this message).This procedure is similar to every other procedure i found online that regards this topic.
The commands i use for the whole procedures are the following:
-)roslaunch mavros px4.launch
-)rosrun mavros mavsafety arm
-)then i run a script to set the OFFBOARD mode (offb_node.cpp, found at the end of the message…I have to use this script since the command “rosrun mavros mavsys mode -c OFFBOARD” it doesn’t work for some reason).
At this point i run the control_actuator script and if i check the values with “rostopic echo mavros/actuator_control” i can see the right numbers for the pins i am interested in.
However the pixhawk outputs are not working accordingly since no PWM signal is present in any output.
Does anyone know how can I fix this?
Thanks you all in advance, let me know if you need other information in order to better understand my problem.
CONTROL_ACTUATOR.CPP:
#include ros/ros.h
#include mavros_msgs/ActuatorControl.h
mavros_msgs::ActuatorControl actuator_control_msg;
void set_actuator_control(){
actuator_control_msg.header.stamp = ros::Time::now();
actuator_control_msg.group_mix = 0;
actuator_control_msg.controls[0] = 0;
actuator_control_msg.controls[1] = 0;
actuator_control_msg.controls[2] = 0.0;
actuator_control_msg.controls[3] = 1.0; //here i try to set throttle pin to 1
actuator_control_msg.controls[4] = 0.0;
actuator_control_msg.controls[5] = 0.0;
actuator_control_msg.controls[6] = 0.0;
actuator_control_msg.controls[7] = 0.0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, “rover_teleop”);
ros::NodeHandle n;
int rate = 10;
ros::Rate loop_rate(10);
//ros::Publisher actuator_controls_pub = n.advertise<mavros_msgs::ActuatorControl("/mavros/actuator_control", 100);
ros::Publisher actuator_controls_pub = n.advertise<mavros_msgs::ActuatorControl>("/mavros/actuator_control", 1000);
while (ros::ok())
{
set_actuator_control();
actuator_controls_pub.publish(actuator_control_msg);
ros::spinOnce();
loop_rate.sleep();
}
}
OFFB_NODE.CPP:
#include ros/ros.h
#include geometry_msgs/PoseStamped.h
#include mavros_msgs/CommandBool.h
#include mavros_msgs/SetMode.h
#include mavros_msgs/State.h
mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *msg;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, “offb_node”);
ros::NodeHandle nh;
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
}
geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i > 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
}
last_request = ros::Time::now();
} else {
if( !current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
}
last_request = ros::Time::now();
}
}
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
}
return 0;
}