From 33c2406577bafc85588ee788a250b428919cad6c Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Wed, 7 Nov 2018 23:20:21 +0000 Subject: [PATCH 1/7] Add skeleton encoder odometry broadcaster --- CMakeLists.txt | 4 ++++ msg/DiffDrive.msg | 3 +++ src/EncOdom/EncOdom.cpp | 17 +++++++++++++++++ src/EncOdom/EncOdom.h | 19 +++++++++++++++++++ 4 files changed, 43 insertions(+) create mode 100644 msg/DiffDrive.msg create mode 100644 src/EncOdom/EncOdom.cpp create mode 100644 src/EncOdom/EncOdom.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e44608..7d65445 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,6 +63,7 @@ add_message_files( Hemisphere.msg RoboClawStats.msg ImuSafety.msg + DiffDrive.msg ) ## Generate services in the 'srv' folder @@ -142,6 +143,7 @@ add_executable( idk src/arm_drone.cpp ) add_executable( road_detection src/road_detection.cpp ) add_executable(imu_safety src/imu_safety/ImuSafety.cpp) add_executable(imu_base src/ImuBase/ImuBase.cpp) +add_executable(enc_odom src/EncOdom/EncOdom.cpp) ## Specify libraries to link a library or executable target against target_link_libraries( Camera ${OpenCV_LIBS} ${catkin_LIBRARIES} ) @@ -154,6 +156,7 @@ target_link_libraries( idk ${catkin_LIBRARIES} ) target_link_libraries( road_detection ${OpenCV_LIBS} ) target_link_libraries(imu_safety ${catkin_LIBRARIES}) target_link_libraries(imu_base ${catkin_LIBRARIES}) +target_link_libraries(enc_odom ${catkin_LIBRARIES}) ## Add cmake target dependencies of the executable @@ -161,6 +164,7 @@ target_link_libraries(imu_base ${catkin_LIBRARIES}) # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(Hemisphere ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) add_dependencies(imu_safety ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(enc_odom ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) #IF(Qt5Widgets_FOUND) diff --git a/msg/DiffDrive.msg b/msg/DiffDrive.msg new file mode 100644 index 0000000..94d1877 --- /dev/null +++ b/msg/DiffDrive.msg @@ -0,0 +1,3 @@ +float64 wheelbase +float64 rvel +float64 lvel diff --git a/src/EncOdom/EncOdom.cpp b/src/EncOdom/EncOdom.cpp new file mode 100644 index 0000000..7e1301a --- /dev/null +++ b/src/EncOdom/EncOdom.cpp @@ -0,0 +1,17 @@ +#include "EncOdom.h" + +EncOdom::EncOdom() + : sub(nh.subscribe("diffdrive_raw", 100, &EncOdom::callback, this)) + , then(ros::Time::now()) +{} + +void EncOdom::callback(const gravl::DiffDrive::ConstPtr& msg) +{ + const auto now = ros::Time::now(); + const auto omega = (msg->rvel - msg->lvel) / msg->wheelbase; + const auto xdot = (msg->rvel + msg->lvel) / 2; +} + +int main(int argc, char** argv) +{ +} diff --git a/src/EncOdom/EncOdom.h b/src/EncOdom/EncOdom.h new file mode 100644 index 0000000..687cb8f --- /dev/null +++ b/src/EncOdom/EncOdom.h @@ -0,0 +1,19 @@ +#pragma once + +#include +#include + +class EncOdom +{ +public: + EncOdom(); + +private: + void callback(const gravl::DiffDrive::ConstPtr& msg); + + ros::NodeHandle nh; + tf2_ros::TransformBroadcaster br; + ros::Subscriber sub; + geometry_msgs::TransformStamped transformStamped; + ros::Time then; +}; From 589b14895768f9212fd5c680d5007e72d9831128 Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Fri, 9 Nov 2018 18:33:55 +0000 Subject: [PATCH 2/7] Complete encoder odometry implementation --- src/EncOdom/EncOdom.cpp | 27 ++++++++++++++++++++++++--- 1 file changed, 24 insertions(+), 3 deletions(-) diff --git a/src/EncOdom/EncOdom.cpp b/src/EncOdom/EncOdom.cpp index 7e1301a..f201672 100644 --- a/src/EncOdom/EncOdom.cpp +++ b/src/EncOdom/EncOdom.cpp @@ -1,17 +1,38 @@ #include "EncOdom.h" +#include EncOdom::EncOdom() : sub(nh.subscribe("diffdrive_raw", 100, &EncOdom::callback, this)) , then(ros::Time::now()) -{} +{ + transformStamped.header.frame_id = "odom"; + transformStamped.child_frame_id = "base_link"; +} void EncOdom::callback(const gravl::DiffDrive::ConstPtr& msg) { const auto now = ros::Time::now(); - const auto omega = (msg->rvel - msg->lvel) / msg->wheelbase; - const auto xdot = (msg->rvel + msg->lvel) / 2; + const auto angularVel = (msg->rvel - msg->lvel) / msg->wheelbase; + const auto tangentialVel = (msg->rvel + msg->lvel) / 2; + const auto dt = (now - then).toSec(); + + tf2::Quaternion deltaAngle; + deltaAngle.setRPY(angularVel * dt, 0, 0); + tf2::Quaternion oldOrientation; + tf2::convert(transformStamped.transform.rotation, oldOrientation); + const auto newOrientation = oldOrientation * deltaAngle; + tf2::convert(oldOrientation * deltaAngle, transformStamped.transform.rotation); + + const auto delta_position = tf2::quatRotate(newOrientation, tf2::Vector3(0, 0, tangentialVel * dt)); + tf2::convert(delta_position, transformStamped.transform.translation); + + br.sendTransform(transformStamped); + then = now; } int main(int argc, char** argv) { + ros::init(argc, argv, "enc_odom"); + EncOdom encOdom; + ros::spin(); } From 4b910ce7ee1fd83a168739a9062e51adc9d13771 Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Fri, 9 Nov 2018 18:47:55 +0000 Subject: [PATCH 3/7] Multiply by RPY instead --- src/EncOdom/EncOdom.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/EncOdom/EncOdom.cpp b/src/EncOdom/EncOdom.cpp index f201672..c1f19b0 100644 --- a/src/EncOdom/EncOdom.cpp +++ b/src/EncOdom/EncOdom.cpp @@ -16,12 +16,10 @@ void EncOdom::callback(const gravl::DiffDrive::ConstPtr& msg) const auto tangentialVel = (msg->rvel + msg->lvel) / 2; const auto dt = (now - then).toSec(); - tf2::Quaternion deltaAngle; - deltaAngle.setRPY(angularVel * dt, 0, 0); tf2::Quaternion oldOrientation; tf2::convert(transformStamped.transform.rotation, oldOrientation); - const auto newOrientation = oldOrientation * deltaAngle; - tf2::convert(oldOrientation * deltaAngle, transformStamped.transform.rotation); + const auto newOrientation = oldOrientation * tf2::Vector3(0, 0, angularVel * dt); + tf2::convert(newOrientation, transformStamped.transform.rotation); const auto delta_position = tf2::quatRotate(newOrientation, tf2::Vector3(0, 0, tangentialVel * dt)); tf2::convert(delta_position, transformStamped.transform.translation); From 090cb6ee54ad487207f75d057d6a724241920900 Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Sat, 10 Nov 2018 19:44:09 +0000 Subject: [PATCH 4/7] Comment the DiffDrive message --- msg/DiffDrive.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/DiffDrive.msg b/msg/DiffDrive.msg index 94d1877..2f1ddd1 100644 --- a/msg/DiffDrive.msg +++ b/msg/DiffDrive.msg @@ -1,3 +1,4 @@ +# This represents the distance between the two wheels float64 wheelbase float64 rvel float64 lvel From 67c3cb5b11d2954ef287493e61802656b4dad93d Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Sat, 10 Nov 2018 19:44:27 +0000 Subject: [PATCH 5/7] Use enc_link as base and actually accumulate translation --- src/EncOdom/EncOdom.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/EncOdom/EncOdom.cpp b/src/EncOdom/EncOdom.cpp index c1f19b0..5c01268 100644 --- a/src/EncOdom/EncOdom.cpp +++ b/src/EncOdom/EncOdom.cpp @@ -6,7 +6,7 @@ EncOdom::EncOdom() , then(ros::Time::now()) { transformStamped.header.frame_id = "odom"; - transformStamped.child_frame_id = "base_link"; + transformStamped.child_frame_id = "enc_link"; } void EncOdom::callback(const gravl::DiffDrive::ConstPtr& msg) @@ -21,8 +21,10 @@ void EncOdom::callback(const gravl::DiffDrive::ConstPtr& msg) const auto newOrientation = oldOrientation * tf2::Vector3(0, 0, angularVel * dt); tf2::convert(newOrientation, transformStamped.transform.rotation); - const auto delta_position = tf2::quatRotate(newOrientation, tf2::Vector3(0, 0, tangentialVel * dt)); - tf2::convert(delta_position, transformStamped.transform.translation); + tf2::Vector3 oldTranslation; + tf2::convert(transformStamped.transform.translation, oldTranslation); + const auto deltaPosition = tf2::quatRotate(newOrientation, tf2::Vector3(0, 0, tangentialVel * dt)); + tf2::convert(oldTranslation + deltaPosition, transformStamped.transform.translation); br.sendTransform(transformStamped); then = now; From 1377be79c342b3baf5f0a0749a9b3a6e12b05ba4 Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Sat, 10 Nov 2018 19:49:28 +0000 Subject: [PATCH 6/7] Use tf2_ros for static transform --- launch/localization.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/localization.launch b/launch/localization.launch index aea7212..06a2ee8 100644 --- a/launch/localization.launch +++ b/launch/localization.launch @@ -10,4 +10,5 @@ + From 1a30cb381cef7eb521e9518aa98f208d5a4291ab Mon Sep 17 00:00:00 2001 From: Kawin Nikomborirak Date: Sat, 10 Nov 2018 19:49:39 +0000 Subject: [PATCH 7/7] Depend on tf2_ros --- CMakeLists.txt | 2 +- package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7d65445..65a274b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS mavros_msgs nav_msgs tf - tf2 + tf2_ros tf2_geometry_msgs cv_bridge ) diff --git a/package.xml b/package.xml index 77a6dbc..88da07a 100644 --- a/package.xml +++ b/package.xml @@ -21,7 +21,7 @@ sensor_msgs std_msgs tf - tf2 + tf2_ros tf2_geometry_msgs rospy