sending velocity commands to the robot throught cmd_vel topic
Hi, I want to send velocity comands to move th bebop drone up and then forward. Here is my code:
#include<ros/ros.h>
#include<nav_msgs/Odometry.h>
#include<geometry_msgs/Twist.h>
#include<stdlib.h>
#include<math.h>
float tag_yaw,tag_x,tag_y,tag_z;
ros::Publisher pub;
ros::Subscriber sub;
geometry_msgs::Twist msg;
float err;
void getpos(const nav_msgs::Odometry& posmsg){
if (posmsg.pose.pose.position.z<= 1){
ROS_INFO_STREAM( "Showing position : "
<< "x1 =" << posmsg.pose.pose.position.x
<< "y1 =" << posmsg.pose.pose.position.y
<< "z1 =" << posmsg.pose.pose.position.z);
msg.linear.x=0.0;
msg.linear.y=0.0;
msg.linear.z= 0.1; // exp ( 0.1);
msg.angular.x= 0;
msg.angular.y= 0;
msg.angular.z=0.0;
}
else {
ROS_INFO_STREAM( "Showing position : "
<< "x =" << posmsg.pose.pose.position.x
<< "y =" << posmsg.pose.pose.position.y
<< "z =" << posmsg.pose.pose.position.z);
msg.linear.x=0.8*(tag_x-posmsg.pose.pose.position.x);
msg.linear.y=0.0;
msg.linear.z= 0.0; // exp ( 0.1);
msg.angular.x= 0;
msg.angular.y= 0;
msg.angular.z=0.0;
}
pub.publish(msg);
}
int main(int argc, char **argv){
ros::init(argc, argv,"velBebop");
ros::NodeHandle nh;
tag_x= .5;
tag_y= 0.0;
tag_z= 0.0;
sub = nh.subscribe("/bebop/odom",100, &getpos) ;
pub = nh.advertise<geometry_msgs::Twist>("/bebop/cmd_vel" ,100);
ros::spin();
return 0;
}
I can see that the velocities are being published through the rqt_graph here:
but the drone is not moving at all! it is not even rotating the helices. Any suggestion!!! Thanks
The code you show does not appear to initialise the
Publisher
at all. Is that a copy-pasta or actual code?@gvdhoorn I forgot that line that I added now. this is my code