From: Walter Fetter Lages Date: Mon, 28 Mar 2022 09:12:35 +0000 (-0300) Subject: twil_ident ported to Galactic. X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=fec73bd8e595faae33f5f5e40e94aa2b3a4d1b28;p=twil.git twil_ident ported to Galactic. --- diff --git a/twil_ident/CMakeLists.txt b/twil_ident/CMakeLists.txt index d44d476..8cde0a7 100644 --- a/twil_ident/CMakeLists.txt +++ b/twil_ident/CMakeLists.txt @@ -1,211 +1,50 @@ -cmake_minimum_required(VERSION 3.0.2) +cmake_minimum_required(VERSION 3.8) project(twil_ident) -## 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 COMPONENTS - kdl_parser - roscpp -) - -find_package(cmake_modules REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(urdf REQUIRED) find_package(Eigen3 REQUIRED) - -## 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 twil_ident -# CATKIN_DEPENDS kdl_parser roscpp -# DEPENDS Eigen3 -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} -# TODO: Check names of system library include directories (Eigen3) - ${EIGEN3_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/twil_ident.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(ident src/ident.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(ident - ${catkin_LIBRARIES} -# ${Eigen3_LIBRARIES} +target_include_directories(ident PUBLIC + $ + $ + $) +target_compile_features(ident PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies( + ident + "rclcpp" + "kdl_parser" + "sensor_msgs" + "std_msgs" + "urdf" + "Eigen3" ) -############# -## 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 ident - 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_twil_ident.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) + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/twil_ident/launch/gazebo.launch b/twil_ident/launch/gazebo.launch deleted file mode 100644 index 80bd7c9..0000000 --- a/twil_ident/launch/gazebo.launch +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/twil_ident/launch/gazebo.launch.xml b/twil_ident/launch/gazebo.launch.xml new file mode 100644 index 0000000..c4a3627 --- /dev/null +++ b/twil_ident/launch/gazebo.launch.xml @@ -0,0 +1,39 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/twil_ident/launch/ident.launch b/twil_ident/launch/ident.launch deleted file mode 100644 index 9af6647..0000000 --- a/twil_ident/launch/ident.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/twil_ident/launch/ident.launch.xml b/twil_ident/launch/ident.launch.xml new file mode 100644 index 0000000..04ee624 --- /dev/null +++ b/twil_ident/launch/ident.launch.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + diff --git a/twil_ident/package.xml b/twil_ident/package.xml index 7000c1e..42299f4 100644 --- a/twil_ident/package.xml +++ b/twil_ident/package.xml @@ -1,69 +1,25 @@ - + + twil_ident - 4.1.0 - The twil_ident package - - - - + 5.0.0 + Indentification of Twil dynamics parameters Walter Fetter Lages - - - - - GPLv3 + ament_cmake - - - - - + rclcpp + kdl_parser + sensor_msgs + std_msgs + urdf + Eigen3 - - - - - Walter Fetter Lages + ament_lint_auto + ament_lint_common - - - - - - - - - - - - - - - - - - - - - - catkin - Eigen3 - kdl_parser - roscpp - Eigen3 - kdl_parser - roscpp - Eigen3 - kdl_parser - roscpp - - - - - + ament_cmake diff --git a/twil_ident/src/ident.cpp b/twil_ident/src/ident.cpp index 88e6fb0..8daef17 100644 --- a/twil_ident/src/ident.cpp +++ b/twil_ident/src/ident.cpp @@ -1,7 +1,7 @@ /****************************************************************************** - ROS twil_indet Package + ROS 2 twil_indet Package Twil Dynamics Model - Copyright (C) 2014..2017 Walter Fetter Lages + Copyright (C) 2014..2022 Walter Fetter Lages This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -19,13 +19,13 @@ *******************************************************************************/ -#include +#include #include -#include -#include -#include +#include +#include +#include #include @@ -59,12 +59,12 @@ Prbs::Prbs(unsigned int n,unsigned int seed) Prbs::~Prbs(void) { -// delete[] sh; + delete[] sh; } Prbs::operator int(void) { - unsigned char s=sh[nd-1]+sh[nd-2]; + auto s=sh[nd-1]+sh[nd-2]; if(s > 1) s=0; for(int j=nd-2;j >= 0;j--) sh[j+1]=sh[j]; sh[0]=s; @@ -72,20 +72,16 @@ Prbs::operator int(void) } -class Ident +class Ident: public rclcpp::Node { public: - Ident(ros::NodeHandle node); - ~Ident(void); - void setCommand(void); + Ident(void); private: - ros::NodeHandle node_; - - ros::Subscriber jointStatesSubscriber; - ros::Publisher dynParamPublisher; - ros::Publisher leftWheelCommandPublisher; - ros::Publisher rightWheelCommandPublisher; + rclcpp::Subscription::SharedPtr jointStatesSubscriber; + rclcpp::Publisher::SharedPtr dynParamPublisher; + rclcpp::Publisher::SharedPtr leftWheelCommandPublisher; + rclcpp::Publisher::SharedPtr rightWheelCommandPublisher; const int nJoints; @@ -98,31 +94,37 @@ class Ident std::vector prbs; int iter; - ros::Time lastTime; + rclcpp::TimerBase::SharedPtr timer; + rclcpp::Time lastTime; + std::string robotDescription; std::vector wheelRadius; double wheelBase; - void jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates); + void setCommandCB(void); + void jointStatesCB(const sensor_msgs::msg::JointState::SharedPtr jointStates); + void robotDescriptionCB(const std_msgs::msg::String::SharedPtr robobotDescription); void resetCovariance(void); }; -Ident::Ident(ros::NodeHandle node): +Ident::Ident(void): Node("twil_ident"), nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),prbs(nJoints),wheelRadius(nJoints) { - node_=node; - - std::string robot_desc_string; - if(!node_.getParam("/robot_description",robot_desc_string)) - { - ROS_ERROR("Could not find '/robot_description'."); - } - + using std::placeholders::_1; + rclcpp::QoS qos(rclcpp::KeepLast(1)); + qos.transient_local(); + auto robotDescriptionSubscriber=create_subscription("robot_description",qos,std::bind(&Ident::robotDescriptionCB,this,_1)); + while(robotDescription.empty()) + { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE(get_logger(),*get_clock(),1000,"Waiting for robot model on /robot_description."); + rclcpp::spin_some(get_node_base_interface()); + } + KDL::Tree tree; - if (!kdl_parser::treeFromString(robot_desc_string,tree)) + if (!kdl_parser::treeFromString(robotDescription,tree)) { - ROS_ERROR("Failed to construct KDL tree."); + RCLCPP_ERROR(get_logger(),"Failed to construct KDL tree."); } // get wheelBase from URDF (actually from KDL tree) @@ -146,23 +148,21 @@ Ident::Ident(ros::NodeHandle node): segmentMapIter=tree.getSegment("right_wheel"); KDL::Joint rightWheelJoint=segmentMapIter->second.segment.getJoint(); wheelRadius[1]=chassisFrame(2,3)+rightSupportFrame(2,3)+rightWheelJoint.JointOrigin().z(); - - jointStatesSubscriber=node_.subscribe("joint_states",1000,&Ident::jointStatesCB,this); - dynParamPublisher=node_.advertise("ident/dynamic_parameters",1000); - leftWheelCommandPublisher=node_.advertise("ident/left_wheel_command",1000); - rightWheelCommandPublisher=node_.advertise("ident/right_wheel_command",1000); + + jointStatesSubscriber=create_subscription("joint_states",1000,std::bind(&Ident::jointStatesCB,this,_1)); + dynParamPublisher=create_publisher("~/dynamic_parameters",1000); + leftWheelCommandPublisher=create_publisher("~/left_wheel_command",1000); + rightWheelCommandPublisher=create_publisher("~/right_wheel_command",1000); u.setZero(); thetaEst1.setZero(); thetaEst2.setZero(); resetCovariance(); - lastTime=ros::Time::now(); -} + using namespace std::chrono_literals; + timer=rclcpp::create_timer(this,this->get_clock(),10ms,std::bind(&Ident::setCommandCB,this)); -Ident::~Ident(void) -{ - jointStatesSubscriber.shutdown(); + lastTime=this->get_clock()->now(); } void Ident::resetCovariance(void) @@ -174,11 +174,12 @@ void Ident::resetCovariance(void) iter=0; } -void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) +void Ident::jointStatesCB(const sensor_msgs::msg::JointState::SharedPtr jointStates) { - ros::Duration dt=jointStates->header.stamp-lastTime; + rclcpp::Duration dt=rclcpp::Time(jointStates->header.stamp)-lastTime; + if(dt.seconds() < 0.01) return; lastTime=jointStates->header.stamp; - + Eigen::VectorXd y=-u; //y(k+1)=(u(k+1)-u(k))/dt Eigen::VectorXd Phi1(nJoints); @@ -198,7 +199,7 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) torque[i]=jointStates->effort[i]; // torque(k) y+=u; - y/=dt.toSec(); + y/=dt.seconds(); Phi1[1]=torque[0]+torque[1]; Phi2[1]=torque[0]-torque[1]; @@ -213,8 +214,8 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) thetaEst2+=K2*(y[1]-yEst2); P2-=K2*Phi2.transpose()*P2; - std_msgs::Float64MultiArray dynParam; - dynParam.layout.dim.push_back(std_msgs::MultiArrayDimension()); + std_msgs::msg::Float64MultiArray dynParam; + dynParam.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); dynParam.layout.dim[0].label="K5 K6 K7 K8 P55 P66 P77 P88"; dynParam.layout.dim[0].size=nJoints*4; dynParam.layout.dim[0].stride=1; @@ -229,35 +230,42 @@ void Ident::jointStatesCB(const sensor_msgs::JointState::ConstPtr &jointStates) dynParam.data.push_back(P1(i,i)); dynParam.data.push_back(P2(i,i)); } - dynParamPublisher.publish(dynParam); + dynParamPublisher->publish(dynParam); // if(iter++ > 2048) resetCovariance(); } -void Ident::setCommand(void) +void Ident::robotDescriptionCB(const std_msgs::msg::String::SharedPtr robotDescription_) +{ + robotDescription=robotDescription_->data; +} + +void Ident::setCommandCB(void) { - std_msgs::Float64 leftCommand; - std_msgs::Float64 rightCommand; - leftCommand.data=10.0*prbs[0]-5.0; - rightCommand.data=10.0*prbs[1]-5.0; - leftWheelCommandPublisher.publish(leftCommand); - rightWheelCommandPublisher.publish(rightCommand); + std_msgs::msg::Float64MultiArray leftCommand; + leftCommand.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + leftCommand.layout.dim[0].label="left wheel command"; + leftCommand.layout.dim[0].size=1; + leftCommand.layout.dim[0].stride=1; + leftCommand.layout.data_offset=0; + leftCommand.data.push_back(10.0*prbs[0]-5.0); + + std_msgs::msg::Float64MultiArray rightCommand; + rightCommand.layout.dim.push_back(std_msgs::msg::MultiArrayDimension()); + rightCommand.layout.dim[0].label="right wheel command"; + rightCommand.layout.dim[0].size=1; + rightCommand.layout.dim[0].stride=1; + rightCommand.layout.data_offset=0; + rightCommand.data.push_back(10.0*prbs[1]-5.0); + + leftWheelCommandPublisher->publish(leftCommand); + rightWheelCommandPublisher->publish(rightCommand); } int main(int argc,char* argv[]) { - ros::init(argc,argv,"twil_ident"); - ros::NodeHandle node; - - Ident ident(node); - - ros::Rate loop(100); - while(ros::ok()) - { - ident.setCommand(); - - ros::spinOnce(); - loop.sleep(); - } + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); return 0; }