-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
+ $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ $<BUILD_INTERFACE:${EIGEN3_INCLUDE_DIR}>
+ $<INSTALL_INTERFACE:include>)
+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()
+++ /dev/null
-<launch>
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
-
- <remap from="/left_wheel_joint_effort_controller/command" to="/left_wheel_command"/>
- <remap from="/right_wheel_joint_effort_controller/command" to="/right_wheel_command"/>
-
- <remap from="/ident/left_wheel_command" to="/left_wheel_command"/>
- <remap from="/ident/right_wheel_command" to="/right_wheel_command"/>
-
- <include file="$(find twil_bringup)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="controller" value="joint_effort_controller"/>
- </include>
-
- <include file="$(find twil_ident)/launch/ident.launch"/>
-</launch>
--- /dev/null
+<!--******************************************************************************
+ Twil Ident
+ Gazebo Launch File
+ Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="pause" default="true"/>
+ <arg name="gui" default="true"/>
+ <arg name="use_sim_time" default="true"/>
+
+ <include file="$(find-pkg-share twil_bringup)/launch/gazebo.launch.xml" >
+ <arg name="pause" value="$(var pause)"/>
+ <arg name="gui" value="$(var gui)"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ <arg name="controller" value="joint_effort_controller"/>
+ </include>
+
+ <include file="$(find-pkg-share twil_ident)/launch/ident.launch.xml">
+ <arg name="left_command_topic" value="/left_wheel_joint_effort_controller/commands"/>
+ <arg name="right_command_topic" value="/right_wheel_joint_effort_controller/commands"/>
+ <arg name="use_sim_time" value="$(var use_sim_time)"/>
+ </include>
+</launch>
+++ /dev/null
-<launch>
- <node name="ident" pkg="twil_ident" type="ident" output="screen" />
-</launch>
--- /dev/null
+<!--******************************************************************************
+ Twil Ident
+ Twil Ident Launch File
+ Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful, but
+ WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Geneal Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************-->
+
+<launch>
+ <arg name="left_command_topic" default="/left_wheel_command"/>
+ <arg name="right_command_topic" default="/right_wheel_command"/>
+ <arg name="use_sim_time" default="true"/>
+
+ <node name="ident" pkg="twil_ident" exec="ident">
+ <remap from="/ident/left_wheel_command" to="$(var left_command_topic)"/>
+ <remap from="/ident/right_wheel_command" to="$(var right_command_topic)"/>
+ <param name="use_sim_time" value="$(var use_sim_time)"/>
+ </node>
+</launch>
<?xml version="1.0"?>
-<package format="2">
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
<name>twil_ident</name>
- <version>4.1.0</version>
- <description>The twil_ident package</description>
-
- <!-- One maintainer tag required, multiple allowed, one person per tag -->
- <!-- Example: -->
- <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <version>5.0.0</version>
+ <description>Indentification of Twil dynamics parameters</description>
<maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
-
-
- <!-- One license tag required, multiple allowed, one license per tag -->
- <!-- Commonly used license strings: -->
- <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>GPLv3</license>
+ <buildtool_depend>ament_cmake</buildtool_depend>
- <!-- Url tags are optional, but multiple are allowed, one per tag -->
- <!-- Optional attribute type can be: website, bugtracker, or repository -->
- <!-- Example: -->
- <!-- <url type="website">http://wiki.ros.org/twil_ident</url> -->
-
+ <depend>rclcpp</depend>
+ <depend>kdl_parser</depend>
+ <depend>sensor_msgs</depend>
+ <depend>std_msgs</depend>
+ <depend>urdf</depend>
+ <depend>Eigen3</depend>
- <!-- Author tags are optional, multiple are allowed, one per tag -->
- <!-- Authors do not have to be maintainers, but could be -->
- <!-- Example: -->
- <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
- <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+ <test_depend>ament_lint_auto</test_depend>
+ <test_depend>ament_lint_common</test_depend>
-
- <!-- The *depend tags are used to specify dependencies -->
- <!-- Dependencies can be catkin packages or system dependencies -->
- <!-- Examples: -->
- <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
- <!-- <depend>roscpp</depend> -->
- <!-- Note that this is equivalent to the following: -->
- <!-- <build_depend>roscpp</build_depend> -->
- <!-- <exec_depend>roscpp</exec_depend> -->
- <!-- Use build_depend for packages you need at compile time: -->
- <!-- <build_depend>message_generation</build_depend> -->
- <!-- Use build_export_depend for packages you need in order to build against this package: -->
- <!-- <build_export_depend>message_generation</build_export_depend> -->
- <!-- Use buildtool_depend for build tool packages: -->
- <!-- <buildtool_depend>catkin</buildtool_depend> -->
- <!-- Use exec_depend for packages you need at runtime: -->
- <!-- <exec_depend>message_runtime</exec_depend> -->
- <!-- Use test_depend for packages you need only for testing: -->
- <!-- <test_depend>gtest</test_depend> -->
- <!-- Use doc_depend for packages you need only for building documentation: -->
- <!-- <doc_depend>doxygen</doc_depend> -->
- <buildtool_depend>catkin</buildtool_depend>
- <build_depend>Eigen3</build_depend>
- <build_depend>kdl_parser</build_depend>
- <build_depend>roscpp</build_depend>
- <build_export_depend>Eigen3</build_export_depend>
- <build_export_depend>kdl_parser</build_export_depend>
- <build_export_depend>roscpp</build_export_depend>
- <exec_depend>Eigen3</exec_depend>
- <exec_depend>kdl_parser</exec_depend>
- <exec_depend>roscpp</exec_depend>
-
-
- <!-- The export tag contains other, unspecified, tags -->
<export>
- <!-- Other tools can request additional information be placed here -->
-
+ <build_type>ament_cmake</build_type>
</export>
</package>
/******************************************************************************
- ROS twil_indet Package
+ ROS 2 twil_indet Package
Twil Dynamics Model
- Copyright (C) 2014..2017 Walter Fetter Lages <w.fetter@ieee.org>
+ Copyright (C) 2014..2022 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
-#include <ros/ros.h>
+#include <rclcpp/rclcpp.hpp>
#include <Eigen/Dense>
-#include <std_msgs/Float64.h>
-#include <std_msgs/Float64MultiArray.h>
-#include <sensor_msgs/JointState.h>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
#include <kdl_parser/kdl_parser.hpp>
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;
}
-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<sensor_msgs::msg::JointState>::SharedPtr jointStatesSubscriber;
+ rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr dynParamPublisher;
+ rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr leftWheelCommandPublisher;
+ rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr rightWheelCommandPublisher;
const int nJoints;
std::vector<Prbs> prbs;
int iter;
- ros::Time lastTime;
+ rclcpp::TimerBase::SharedPtr timer;
+ rclcpp::Time lastTime;
+ std::string robotDescription;
std::vector<double> 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<std_msgs::msg::String>("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)
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<std_msgs::Float64MultiArray>("ident/dynamic_parameters",1000);
- leftWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/left_wheel_command",1000);
- rightWheelCommandPublisher=node_.advertise<std_msgs::Float64>("ident/right_wheel_command",1000);
+
+ jointStatesSubscriber=create_subscription<sensor_msgs::msg::JointState>("joint_states",1000,std::bind(&Ident::jointStatesCB,this,_1));
+ dynParamPublisher=create_publisher<std_msgs::msg::Float64MultiArray>("~/dynamic_parameters",1000);
+ leftWheelCommandPublisher=create_publisher<std_msgs::msg::Float64MultiArray>("~/left_wheel_command",1000);
+ rightWheelCommandPublisher=create_publisher<std_msgs::msg::Float64MultiArray>("~/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)
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);
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];
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;
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<Ident>());
+ rclcpp::shutdown();
return 0;
}
projects / twil.git / commitdiff