diff --git a/.catkin_workspace b/.catkin_workspace new file mode 100644 index 0000000..52fd97e --- /dev/null +++ b/.catkin_workspace @@ -0,0 +1 @@ +# This file currently only serves to mark the location of a catkin workspace for tool integration diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..eea29b3 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,21 @@ +{ + "configurations": [ + { + "browse": { + "databaseFilename": "${default}", + "limitSymbolsToIncludedHeaders": false + }, + "includePath": [ + "/opt/ros/noetic/include/**", + "/home/sasu/kubot_ws/src/kubot_description/include/**", + "/usr/include/**" + ], + "name": "ROS", + "intelliSenseMode": "gcc-x64", + "compilerPath": "/usr/bin/gcc", + "cStandard": "gnu11", + "cppStandard": "c++14" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..d1fc194 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,8 @@ +{ + "python.autoComplete.extraPaths": [ + "/opt/ros/noetic/lib/python3/dist-packages" + ], + "python.analysis.extraPaths": [ + "/opt/ros/noetic/lib/python3/dist-packages" + ] +} \ No newline at end of file diff --git a/src/turtlebot_controller/CMakeLists.txt b/src/turtlebot_controller/CMakeLists.txt index ab72574..401031b 100644 --- a/src/turtlebot_controller/CMakeLists.txt +++ b/src/turtlebot_controller/CMakeLists.txt @@ -6,6 +6,10 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs geometry_msgs + sensor_msgs + nav_msgs + tf2 + tf2_ros ) find_package(Eigen3 REQUIRED) @@ -14,6 +18,10 @@ INCLUDE_DIRS include CATKIN_DEPENDS std_msgs geometry_msgs +sensor_msgs +nav_msgs +tf2 +tf2_ros DEPENDS Eigen3 # DEPENDS system_lib ) diff --git a/src/turtlebot_controller/include/turtlebot_controller/simple_controller.h b/src/turtlebot_controller/include/turtlebot_controller/simple_controller.h index c64096a..13b37f8 100644 --- a/src/turtlebot_controller/include/turtlebot_controller/simple_controller.h +++ b/src/turtlebot_controller/include/turtlebot_controller/simple_controller.h @@ -1,18 +1,44 @@ #ifndef SIMPLE_CONTROLLER_H #define SIMPLE_CONTROLLER_H + #include #include #include +#include +#include +#include + + class SimpleController { - public: - SimpleController(const ros::NodeHandle & ,double radius, double seperation); - private: - void velCallBack(const geometry_msgs::Twist &); +public: + SimpleController(const ros::NodeHandle &, double radius, double separation); + +private: + void velCallback(const geometry_msgs::Twist &); + + void jointCallback(const sensor_msgs::JointState &); + ros::NodeHandle nh_; ros::Subscriber vel_sub_; ros::Publisher right_cmd_pub_; ros::Publisher left_cmd_pub_; + ros::Subscriber joint_sub_; + ros::Publisher odom_pub_; + + // Differential Kinematics Eigen::Matrix2d speed_conversion_; + + // Odometry + double wheel_radius_; + double wheel_separation_; + double right_wheel_prev_pos_; + double left_wheel_prev_pos_; + ros::Time prev_time_; + nav_msgs::Odometry odom_msg_; + double x_; + double y_; + double theta_; + geometry_msgs::TransformStamped transform_stamped_; }; -#endif \ No newline at end of file +#endif \ No newline at end of file diff --git a/src/turtlebot_controller/nodes/simple_controller_node.cpp b/src/turtlebot_controller/nodes/simple_controller_node.cpp index b5aa864..22c845c 100644 --- a/src/turtlebot_controller/nodes/simple_controller_node.cpp +++ b/src/turtlebot_controller/nodes/simple_controller_node.cpp @@ -1,15 +1,17 @@ #include #include "turtlebot_controller/simple_controller.h" -int main(int argc, char **argv){ + +int main(int argc, char **argv) +{ ros::init(argc, argv, "simple_controller"); ros::NodeHandle nh; ros::NodeHandle pnh("~"); - double wheel_radius; + double wheel_radius, wheel_separation; pnh.getParam("wheel_radius", wheel_radius); - double wheel_seperation; - pnh.getParam("wheel_seperation", wheel_seperation); - SimpleController controller(nh, wheel_radius, wheel_seperation); + pnh.getParam("wheel_separation", wheel_separation); + SimpleController controller(nh, wheel_radius, wheel_separation); ros::spin(); + return 0; } \ No newline at end of file diff --git a/src/turtlebot_controller/package.xml b/src/turtlebot_controller/package.xml index 7eb9ea1..5150af7 100644 --- a/src/turtlebot_controller/package.xml +++ b/src/turtlebot_controller/package.xml @@ -54,14 +54,26 @@ std_msgs geometry_msgs eigen + sensor_msgs + nav_msgs + tf2 + tf2_ros roscpp rospy eigen + sensor_msgs + nav_msgs + tf2 + tf2_ros roscpp rospy std_msgs geometry_msgs eigen + sensor_msgs + nav_msgs + tf2 + tf2_ros diff --git a/src/turtlebot_controller/src/simple_controller.cpp b/src/turtlebot_controller/src/simple_controller.cpp index 74eefce..8681c91 100644 --- a/src/turtlebot_controller/src/simple_controller.cpp +++ b/src/turtlebot_controller/src/simple_controller.cpp @@ -1,29 +1,116 @@ -#include +#include "turtlebot_controller/simple_controller.h" #include #include +#include +#include -SimpleController::SimpleController(const ros::NodeHandle &nh ,double radius, double seperation): nh_(nh) + +SimpleController::SimpleController(const ros::NodeHandle &nh, + double radius, + double separation) + : nh_(nh) + , wheel_radius_(radius) + , wheel_separation_(separation) + , left_wheel_prev_pos_(0.0) + , right_wheel_prev_pos_(0.0) + , x_(0.0) + , y_(0.0) + , theta_(0.0) { - ROS_INFO_STREAM("Using wheel raduis" << radius); - ROS_INFO_STREAM("Using wheel seperation" << seperation); + ROS_INFO_STREAM("Using wheel radius " << radius); + ROS_INFO_STREAM("Using wheel separation " << separation); right_cmd_pub_ = nh_.advertise("wheel_right_controller/command", 10); left_cmd_pub_ = nh_.advertise("wheel_left_controller/command", 10); + vel_sub_ = nh_.subscribe("turtlebot_controller/cmd_vel", 1000, &SimpleController::velCallback, this); + joint_sub_ = nh_.subscribe("joint_states", 1000, &SimpleController::jointCallback, this); + odom_pub_ = nh_.advertise("turtlebot_controller/odom", 10); - vel_sub_ = nh_.subscribe("turtlebot_controller/cmd.vel", 1000, &SimpleController::velCallBack, this); - speed_conversion_ < + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/turtlebot_description/urdf/turtlebot.urdf.xacro b/src/turtlebot_description/urdf/turtlebot.urdf.xacro index 39fb122..8af799e 100644 --- a/src/turtlebot_description/urdf/turtlebot.urdf.xacro +++ b/src/turtlebot_description/urdf/turtlebot.urdf.xacro @@ -23,7 +23,7 @@ - + @@ -48,7 +48,7 @@ - + @@ -93,8 +93,29 @@ +<<<<<<< HEAD <<<<<<< HEAD +======= + + + + + + + + + + + + + + + + ======= >>>>>>> devolop @@ -164,8 +185,12 @@ izz="0.001" /> +<<<<<<< HEAD <<<<<<< HEAD +======= + - + +<<<<<<< HEAD <<<<<<< HEAD ======= --> >>>>>>> devolop +======= + +>>>>>>> c06338a682c5cc259374cb313033cea656328708 @@ -205,7 +234,7 @@ - + + + + + + ======= @@ -238,30 +273,38 @@ +<<<<<<< HEAD <<<<<<< HEAD ======= + - + >>>>>>> devolop +======= + --> +>>>>>>> c06338a682c5cc259374cb313033cea656328708 \ No newline at end of file diff --git a/src/turtlebot_description/urdf/turtlebot_gazebo.xacro b/src/turtlebot_description/urdf/turtlebot_gazebo.xacro index 112e455..3e7f4eb 100644 --- a/src/turtlebot_description/urdf/turtlebot_gazebo.xacro +++ b/src/turtlebot_description/urdf/turtlebot_gazebo.xacro @@ -32,18 +32,7 @@ Gazebo/FlatBlack - - 0.1 - 0.1 - 1000000 - 100 - 0.001 - 1 - 1 0 0 - Gazebo/FlatBlack - - - + 0.1 0.1 1000000 @@ -82,4 +71,33 @@ hardware_interface/VelocityJointInterface + + + + + 0 0 0 0 0 0 + true + 10 + + + + 360 + -3.14 + 3.14 + + + + 0.3 + 12 + + + + + ~/out:=scan + + sensor_msgs/LaserScan + laser_frame + + + \ No newline at end of file diff --git a/src/turtlebot_slam/CMakeLists.txt b/src/turtlebot_slam/CMakeLists.txt new file mode 100644 index 0000000..968bf81 --- /dev/null +++ b/src/turtlebot_slam/CMakeLists.txt @@ -0,0 +1,202 @@ +cmake_minimum_required(VERSION 3.0.2) +project(turtlebot_slam) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare and build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES turtlebot_slam +# CATKIN_DEPENDS other_catkin_pkg +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include +# ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/turtlebot_slam.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide +# add_executable(${PROJECT_NAME}_node src/turtlebot_slam_node.cpp) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +# target_link_libraries(${PROJECT_NAME}_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_turtlebot_slam.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/src/turtlebot_slam/config/turtlebot_lds_2d.lua b/src/turtlebot_slam/config/turtlebot_lds_2d.lua new file mode 100644 index 0000000..117804f --- /dev/null +++ b/src/turtlebot_slam/config/turtlebot_lds_2d.lua @@ -0,0 +1,58 @@ +-- Copyright 2016 The Cartographer Authors +-- +-- 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. + +include "map_builder.lua" +include "trajectory_builder.lua" + +options = { + map_builder = MAP_BUILDER, + trajectory_builder = TRAJECTORY_BUILDER, + map_frame = "map", +-- tracking_frame = "imu_link", -- imu_link, If you are using gazebo, use 'base_footprint' (libgazebo_ros_imu's bug) + tracking_frame = "base_footprint" + published_frame = "odom", + odom_frame = "odom", + provide_odom_frame = false, + publish_frame_projected_to_2d = false, + use_odometry = true, + use_nav_sat = false, + use_landmarks = false, + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, + num_subdivisions_per_laser_scan = 1, + num_point_clouds = 0, + lookup_transform_timeout_sec = 0.2, + submap_publish_period_sec = 0.3, + pose_publish_period_sec = 5e-3, + trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 0.1, + fixed_frame_pose_sampling_ratio = 1., + imu_sampling_ratio = 0.1, + landmarks_sampling_ratio = 1., +} + +MAP_BUILDER.use_trajectory_builder_2d = true + +TRAJECTORY_BUILDER_2D.min_range = 0.1 +TRAJECTORY_BUILDER_2D.max_range = 3.5 +TRAJECTORY_BUILDER_2D.missing_data_ray_length = 3. +TRAJECTORY_BUILDER_2D.use_imu_data = true +TRAJECTORY_BUILDER_2D.use_online_correlative_scan_matching = true +TRAJECTORY_BUILDER_2D.motion_filter.max_angle_radians = math.rad(0.1) + +POSE_GRAPH.constraint_builder.min_score = 0.65 +POSE_GRAPH.constraint_builder.global_localization_min_score = 0.7 + +return options \ No newline at end of file diff --git a/src/turtlebot_slam/package.xml b/src/turtlebot_slam/package.xml new file mode 100644 index 0000000..7238a3d --- /dev/null +++ b/src/turtlebot_slam/package.xml @@ -0,0 +1,59 @@ + + + turtlebot_slam + 0.0.0 + The turtlebot_slam package + + + + + sasu + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + + + + + + + +