71 lines
2.1 KiB
C++
71 lines
2.1 KiB
C++
// Copyright 2019 ROBOTIS CO., LTD.
|
|
//
|
|
// Licensed under the Apache License, Version 2.0 (the "License");
|
|
// you may not use this file except in compliance with the License.
|
|
// You may obtain a copy of the License at
|
|
//
|
|
// http://www.apache.org/licenses/LICENSE-2.0
|
|
//
|
|
// Unless required by applicable law or agreed to in writing, software
|
|
// distributed under the License is distributed on an "AS IS" BASIS,
|
|
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
// See the License for the specific language governing permissions and
|
|
// limitations under the License.
|
|
//
|
|
// Authors: Taehun Lim (Darby), Ryan Shim
|
|
|
|
#ifndef TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
|
|
#define TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
|
|
|
|
#include <tf2/LinearMath/Matrix3x3.h>
|
|
#include <tf2/LinearMath/Quaternion.h>
|
|
#include <geometry_msgs/msg/twist.hpp>
|
|
#include <nav_msgs/msg/odometry.hpp>
|
|
#include <rclcpp/rclcpp.hpp>
|
|
#include <sensor_msgs/msg/laser_scan.hpp>
|
|
|
|
#define DEG2RAD (M_PI / 180.0)
|
|
#define RAD2DEG (180.0 / M_PI)
|
|
|
|
#define CENTER 0
|
|
#define LEFT 1
|
|
#define RIGHT 2
|
|
|
|
#define LINEAR_VELOCITY 0.3
|
|
#define ANGULAR_VELOCITY 1.5
|
|
|
|
#define GET_TB3_DIRECTION 0
|
|
#define TB3_DRIVE_FORWARD 1
|
|
#define TB3_RIGHT_TURN 2
|
|
#define TB3_LEFT_TURN 3
|
|
|
|
class Turtlebot3Drive : public rclcpp::Node
|
|
{
|
|
public:
|
|
Turtlebot3Drive();
|
|
~Turtlebot3Drive();
|
|
|
|
private:
|
|
// ROS topic publishers
|
|
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
|
|
|
|
// ROS topic subscribers
|
|
rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_sub_;
|
|
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
|
|
|
// Variables
|
|
double robot_pose_;
|
|
double prev_robot_pose_;
|
|
double scan_data_[3];
|
|
|
|
// ROS timer
|
|
rclcpp::TimerBase::SharedPtr update_timer_;
|
|
|
|
// Function prototypes
|
|
void update_callback();
|
|
void update_cmd_vel(double linear, double angular);
|
|
void scan_callback(const sensor_msgs::msg::LaserScan::SharedPtr msg);
|
|
void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
|
|
};
|
|
#endif // TURTLEBOT3_GAZEBO__TURTLEBOT3_DRIVE_HPP_
|