差速轮由轮子解算里程计

void get_odom(struct Odom* odom, float* motor_dis, unsigned long interval)
{
float dxy_ave = (motor_dis[0] + motor_dis[1]) / 2.0;
float dth = (-motor_dis[0] + motor_dis[1]) / (2* body_radius);
float vxy = 1000 * dxy_ave / interval;
float vth = 1000 * dth / interval;

	odom->vel_x = vxy;
	odom->vel_y = 0;
	odom->vel_z = vth;
	float dx = 0, dy = 0;
	
		dx = cos(dth) * dxy_ave;
		dy = -sin(dth) * dxy_ave;
		odom->x += (cos(odom->z) * dx - sin(odom->z) * dy);
		odom->y += (sin(odom->z) * dx + cos(odom->z) * dy);;
	
		odom->z += dth;
}

void LiftRobotNode::publish_timer(const std::chrono::microseconds timeout)
{
dxl=0;dxr=0;yaw=0;
publish_timer_ = this->create_wall_timer(
timeout,
this -> void
{
rclcpp::Time now = this->now();
read (*sp,buffer(uart6_rx_buff));

    if((USART6_RX_STA & 0x40) != 0) 
      {
       if((USART6_RX_STA & 0x80) == 0)
        {
         if(uart6_rx_buff[0] == 0xef)
          USART6_RX_STA |=0x80;  
        else 
          {
           uart6_deal_buff[USART6_RX_STA & 0X3F] = uart6_rx_buff[0];
           USART6_RX_STA++;
           if((USART6_RX_STA & 0X3F) > 13)
            USART6_RX_STA = 0;
          }
        }
      }
      else if(uart6_rx_buff[0] == 0xee)   
            USART6_RX_STA |= 0x40;

      if(((USART6_RX_STA & 0x80) != 0)) //一帧数据接受完毕处理数据
      {
       framecnt++;
       DecodeS32To4ByteNone(&MotorPosition[0],&uart6_deal_buff[0])
       DecodeS32To4ByteNone(&MotorPosition[1],&uart6_deal_buff[4])
       //DecodeS16To2ByteNone(&MotorSpeed[0],&uart6_deal_buff[8])
       //DecodeS16To2ByteNone(&MotorSpeed[1],&uart6_deal_buff[10])
       //DecodeS16To2ByteNone(&MotorCurrent[0],&uart6_deal_buff[12])
       //DecodeS16To2ByteNone(&MotorCurrent[1],&uart6_deal_buff[14])
       DecodeS32To4ByteNone(&checksum,&uart6_deal_buff[8])
       /*if(checksum == (MotorPosition[0]+MotorPosition[1]+MotorSpeed[0]+MotorSpeed[1]+MotorCurrent[0]+MotorCurrent[1]))*/
       if (checksum == (MotorPosition[0] + MotorPosition[1]))
       {
        RCLCPP_INFO(get_logger(), "checksum pass %d %d %d %d %d",MotorPosition[0],MotorPosition[1],checksum,framecnt,errocnt);
        xl=MotorPosition[0]*3.1415926*wheel_dia/4096;  //左轮行进距离
        xr=MotorPosition[1]*3.1415926*wheel_dia/4096;  //右轮行进距离

        if(!init) {xl_last=xl;xr_last=xr;init=true;}

        dxl=xl-xl_last;
        dxr=xr-xr_last;
        xr_last=xr;
        xl_last=xl;
        double dxy_ave = (dxl+ dxr) / 2.0;
        double dth = (-dxl+dxr) /car_width;
        double vxy =  dxy_ave / 10 *1000;
        double vth =  dth / 10 *1000;

        odom.twist.twist.linear.x= vxy;      
        odom.twist.twist.angular.z  = vth;

        float dx = 0, dy = 0;
        dx = cos(dth) * dxy_ave;
        dy = -sin(dth) * dxy_ave;
        odom.pose.pose.position.x += (cos(yaw) * dx - sin(yaw) * dy);
        odom.pose.pose.position.y += (sin(yaw) * dx + cos(yaw) * dy);;          
        yaw += dth;
        
        tf2::Quaternion q;
        q.setRPY(0.0, 0.0, yaw);

        odom_trans.header.stamp =  clock.now();  
        odom_trans.transform.translation.x = odom.pose.pose.position.x;  
        odom_trans.transform.translation.y = odom.pose.pose.position.y;  
        odom_trans.transform.rotation.x = q.x();
        odom_trans.transform.rotation.y = q.y();
        odom_trans.transform.rotation.z = q.z();
        odom_trans.transform.rotation.w= q.w();
        odom_broadcaster->sendTransform(odom_trans); 

        odom.header.stamp = clock.now();   
        //odom.pose.pose.orientation = odom_quat;  
        odom.pose.pose.orientation.x = q.x();
        odom.pose.pose.orientation.y = q.y();
        odom.pose.pose.orientation.z = q.z();
        odom.pose.pose.orientation.w = q.w();
        odom_pub->publish(odom);
       }
       else
       {
        errocnt++;
        RCLCPP_INFO(get_logger(), "checksum failed  %d %d %d %d %d",MotorPosition[0],MotorPosition[1],checksum,framecnt,errocnt);
       }

       USART6_RX_STA=0;
       memset(uart6_deal_buff,0,sizeof(uart6_deal_buff));
      }	
  }
);

}

发布了13 篇原创文章 · 获赞 0 · 访问量 757
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览