ROS 모터 센서 뷰어 통합

앞서 공부한 odometry, lidar, imu를 활용하야 rviz 상에서 8자로 동작하는 모형을 구현한다.

 

rviz_all
│   ├── CMakeLists.txt
│   ├── include
│   │   └── rviz_all
│   ├── launch
│   │   └── rviz_all.launch
│   ├── package.xml
│   ├── rviz
│   │   └── rviz_all.rviz
│   ├── src
│   │   └── odom_imu.cpp
│   └── urdf
│       └── rviz_all.urdf

이외의 필요한 것은 전에 사용했던 파일을 사용한다.

 

#include "ros/init.h"
#include "ros/node_handle.h"
#include "ros/publisher.h"
#include "ros/subscriber.h"
#include "ros/time.h"
#include "tf/LinearMath/Quaternion.h"
#include <iterator>
#include <ros/ros.h>
#include <tf/tf.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <cmath>
#include <vector>
#include <algorithm>

using namespace std;
using namespace ros;

double angle;
vector<float> imu_data;

void callback(const sensor_msgs::JointState::ConstPtr& data){
    auto ptr = find(data->name.begin(), data->name.end(), "front_left_hinge_joint");
    int index = distance(data->name.begin(), ptr);
    angle = data->position[index];
}

void callback_imu(const sensor_msgs::Imu::ConstPtr& data){
    imu_data.push_back(data->orientation.x);
    imu_data.push_back(data->orientation.y);
    imu_data.push_back(data->orientation.z);
    imu_data.push_back(data->orientation.w);
}

int main(int argc, char** argv){
    
    ros::init(argc, argv, "odometry_publisher");

    NodeHandle nh;

    Subscriber sub = nh.subscribe("joint_states",1,callback);
    Subscriber sub_imu = nh.subscribe("imu",1,callback_imu);
    tf2::Quaternion quaternion;
    quaternion.setRPY(0, 0, 0);

    Publisher odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broad;

    Time current_time = Time::now();
    Time last_time = Time::now();

    Rate r(30);

    double current_speed = 0.4;
    double wheel_base = 0.2;
    double x_ =0;
    double y_ =0;
    double yaw_ =0;
    double Angle_ =0;

    while(ok()){
        current_time = Time::now();

        double dt = (current_time-last_time).toSec();
        double current_steering_angle = angle;
        double current_angular_velocity = current_speed * tan(current_steering_angle)/wheel_base;

        double x_dot = current_speed *cos(yaw_) ;
        double y_dot = current_speed *sin(yaw_);
        x_ += x_dot *dt;
        y_ += y_dot *dt;
        yaw_ += current_angular_velocity *dt;

        quaternion.setRPY(0, 0, yaw_);

        tf::StampedTransform odom_trans;
        odom_trans.stamp_ = current_time;
        odom_trans.frame_id_ = "base_link";
        odom_trans.child_frame_id_ = "odom";
        odom_trans.setOrigin(tf::Vector3(x_, y_, 0));
        odom_trans.setRotation(tf::Quaternion(quaternion.getX(),quaternion.getY(),quaternion.getZ(),quaternion.getW()));

        odom_broad.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.;

        odom.pose.pose.orientation.x = quaternion.x();
        odom.pose.pose.orientation.y = quaternion.y();
        odom.pose.pose.orientation.z = quaternion.z();
        odom.pose.pose.orientation.w = quaternion.w();

        odom.child_frame_id = "base_link";

        odom_pub.publish(odom);

        last_time = current_time;

        r.sleep();
    }

}