用自己的机器人和ubuntu PC实现通信和控制

前端之家收集整理的这篇文章主要介绍了用自己的机器人和ubuntu PC实现通信和控制前端之家小编觉得挺不错的,现在分享给大家,也给大家做个参考。

前提: 1.拥有一台能够采用手动或者自动移动的机器人移动平台。

    2.在电机端需要安装高分辨率的霍尔编码器。

    3.在终端控制板上有基本的电机PWM控制,PID电机补偿调速。编码器输入捕获,串口数据收发的驱动。

以下是我的机器人:

控制思路如下:

1.电机驱动板获取编码器的数据通过CAN总线透传给地盘主控。地盘主控解析编码器的数据计算出X方向上的线速度和Z轴上的角速度,通过串口上传到ubuntuPC机上。源码如下:(采用定时器方式计算速度)

struct Encoder_{ float d_left; float d_right;int enc_left; //wheel encoder readings int enc_right; int left; // actual values coming back from robot int right; }self; float d,th; #define ticks_meter 123077.0 //每米编码器的值 linear = 2.6 #define base_width 0.31f; //轴距 angule = 0.316 #define robot_timer 0.53f //周期  union Max_Value { unsigned char buf[12]; struct _Float_{        float hander; float _float_vx; float _float_vth; }Float_RAM; }Send_Data; extern int encoder_0; extern int encoder_1; void Updata_velocities_Data() {    u8 i=0; self.right = encoder_0;//编码器的累计量 self.left = encoder_1; if(self.enc_left == 0) { self.d_left = 0; self.d_right = 0; } else { self.d_left = (self.left - self.enc_left) / ticks_meter; self.d_right = (self.right - self.enc_right) / ticks_meter; } self.enc_left = self.left; self.enc_right = self.right;  d = ( self.d_left + self.d_right ) / 2; //distance traveled is the average of the two wheels th = ( self.d_right - self.d_left ) / base_width; //this approximation works (in radians) for small angles  self.dx = d / robot_timer; //calculate velocities self.dr = th / robot_timer;    Send_Data.Float_RAM.hander = 15.5; Send_Data.Float_RAM._float_vx = self.dx; Send_Data.Float_RAM._float_vth = self.dr; for(i=0;i<12;i++)     sendchar_usart1(Send_Data.buf[i]); }

  2.在ubuntu PC端通过boost串口接收数据时。对串口进行读取 即可。在此节点中我们需要发布从下位机获取的速度信息,还要监听/cmd_vel话题下的移动数据,发送到下位机。代码如下:

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <iostream>
#include <iomanip>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <math.h>

using namespace std;
using namespace boost::asio;

ros::Time current_time,last_time;
double x = 0.0;
double y = 0.0;
double th = 0.0;
double vx = 0.0;
double vy = 0.0;
double vth = 0.0;

union _SPEED_
{
 unsigned char speed_buf[16];
 struct _speed_value_
 {
  float flag;
  float left_vel;
  float yspeed_vel;
  float right_vel;
 }Struct_Speed;
}Union_Speed;

geometry_msgs::Quaternion odom_quat;

void cmd_velCallback(const geometry_msgs::Twist &twist_aux)
{
 geometry_msgs::Twist twist = twist_aux;
 
 Union_Speed.Struct_Speed.flag = 15.5;
 Union_Speed.Struct_Speed.left_vel = twist_aux.linear.x;
 Union_Speed.Struct_Speed.yspeed_vel = 0.0;
 Union_Speed.Struct_Speed.right_vel = twist_aux.angular.z;
}

double dt = 0.0; 

union _Data_
{
 unsigned char buf_rev[12];
 struct _value_encoder_
 {
  float Flag_Float;
  float VX_speed;
  float VZ_speed;
 }Struct_Encoder;
}Union_Data;

int main(int argc,char** argv)
{
 unsigned char check_buf[1];
 unsigned char i;
 io_service iosev;
 serial_port sp(iosev,"/dev/ttyUSB0");
 sp.set_option(serial_port::baud_rate(115200));
 sp.set_option(serial_port::flow_control(serial_port::flow_control::none));
 sp.set_option(serial_port::parity(serial_port::parity::none));
 sp.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
 sp.set_option(serial_port::character_size(8));

 ros::init(argc,argv,"base_controller");
 ros::NodeHandle n;
 ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel",50,cmd_velCallback);
 ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom",50);
  tf::TransformBroadcaster odom_broadcaster;
 
 while(ros::ok())
 {
  current_time = ros::Time::now();
  
  ros::spinOnce(); 
  dt = (current_time - last_time).toSec(); 
  last_time = ros::Time::now();
   
  current_time = ros::Time::now();
  read(sp,buffer(Union_Data.buf_rev));
  
  if(Union_Data.Struct_Encoder.Flag_Float == 15.5)
  {
    vx = Union_Data.Struct_Encoder.VX_speed;
    vth = Union_Data.Struct_Encoder.VZ_speed;
          ROS_INFO("msg_encoder.angular_z is %f",vth);
          ROS_INFO("msg_encoder.linear.x is %f",vx);   }   else   {     ROS_INFO("Fucking communication fails,The fuck can i hurry up to restart!");    read(sp,buffer(check_buf));   }    write(sp,buffer(Union_Speed.speed_buf,16));      double delta_x = (vx * cos(th) - vy * sin(th)) * dt;   double delta_y = (vx * sin(th) + vy * cos(th)) * dt;   double delta_th = vth * dt;   x += delta_x;   y += delta_y;   th += delta_th;      geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);      geometry_msgs::TransformStamped odom_trans;   odom_trans.header.stamp = current_time;   odom_trans.header.frame_id = "odom";   odom_trans.child_frame_id = "base_link";   odom_trans.transform.translation.x = x;   odom_trans.transform.translation.y = y;   odom_trans.transform.translation.z = 0.0;   odom_trans.transform.rotation = odom_quat;      odom_broadcaster.sendTransform(odom_trans);      nav_msgs::Odometry odom;   odom.header.stamp = current_time;   odom.header.frame_id = "odom";      odom.pose.pose.position.x = x;   odom.pose.pose.position.y = y;   odom.pose.pose.position.z = 0.0;   odom.pose.pose.orientation = odom_quat;      odom.child_frame_id = "base_link";   odom.twist.twist.linear.x = vx;   odom.twist.twist.linear.y = vy;   odom.twist.twist.angular.z = vth;      odom_pub.publish(odom);   last_time = current_time;  }  iosev.run(); } 

然后测试如下:打开你的机器人,和ubuntu PC 确保串口线链接。然后运行所需的节点:

roscore &
rosrun odom_tf_package tf_broadcaster_node
rviz
rosrun rbx1_nav timed_out_and_back.py  //此节点为turtlebot的damo

在tf_broadcaster_node节点下的调试输出如下:

rviz视图中查看机器人入下:

原文地址

http://www.cnblogs.com/zxouxuewei/p/5527823.html

猜你在找的Ubuntu相关文章