diff --git a/ROS-Driver-Update/LICENSE b/LICENSE similarity index 100% rename from ROS-Driver-Update/LICENSE rename to LICENSE diff --git a/ROS-Driver-Update/README.md b/README.md similarity index 55% rename from ROS-Driver-Update/README.md rename to README.md index 980ef41..f7c7670 100644 --- a/ROS-Driver-Update/README.md +++ b/README.md @@ -1,5 +1,8 @@ # ROS-Driver -# Quick Start Guide + +## Introduction +**From Roboteq** + ``` Note* -This ROS driver only supports firmware version 2.0 or 2.0+. @@ -7,20 +10,9 @@ -If firmware is not the latest one then please update it with the latest one available on Roboteq website or contact "techsupport.roboteq@mail.nidec.com". ``` -This repository contains the ROS driver for Roboteq controllers. The package requires ROS system to be installed properly to your system and proper connection of Roboteq controller. For detailed controller setup instructions, please refer to our documentation [here](https://www.roboteq.com/index.php/docman/motor-controllers-documents-and-files/documentation/user-manual/272-roboteq-controllers-user-manual-v17/file). - -First, clone this repository to catkin_ws/src -``` -git clone https://github.com/Roboteq-Inc/ROS-Driver.git -``` - -The `Roboteq motor controller driver` is the package for the Roboteq ROS-driver. Make sure not to change package name as it will change the definition in the Cmake and Package files. Open new terminal and copy these steps - +This repository contains the ROS driver for Roboteq controllers. The package requires ROS system to be installed properly to your system and proper connection of Roboteq controller. For detailed controller setup instructions, please refer to our documentation [here (link died)](?). -``` -cd catkin_ws/ -source devel/setup.bash -roslaunch roboteq_motor_controller_driver driver.launch -``` +**Roboteq CLAIMED!!!** The roboteq driver is designed to be dynamic and users can publish the controller queries as per their requirements. The publishing queries is not limited to any value. By default total 9 queries are published by launching this driver. Users can change or add queries in configuration file. For that go to config/query.yaml @@ -47,3 +39,23 @@ queryG: # status_flag : ?FS # firmware_id : ?FID Users can add queries which do not require channel number under queryG tab. ``` + +**FACTS:** +- It originally worked at a fixed rate of 5 Hz while querying system's states. I did **major** mofifications in this work, making it work at a frequency you want to. +- They did also specified 3 separate query frequencies in [the config file](config/query.yaml): `frequencyH`, `frequencyL`, and `frequencyG`. However, it's not the case (or quite complicated). Here in this work, I cleaned all of it and only keep a default `frequency` for all queries. It's sufficient for me, and hopefully for you too. +- I only used the `driver.launch` for now. So `diff_odom` is kept as original. Later, I might make it works, but probably by modifying the `roboteq_controller_node`, not putting in a separated file to make it a little bit efficent. + + +**Note**: This package is tested on XDC2460. In general, it uses serial communication so feel free to test it on your roboteq device. Please let me know it you do so, whether it works, any issues. Thank you. + + + +## Installation +```bash +cd YOUR_WS/src/ +git clone https://github.com/DoanNguyenTrong/roboteq_controller_ros.git +cd YOUR_WS +colcon build --packages-select roboteq_controller +. install/setup.bash +ros2 launch roboteq_controller roboteq_launch.py +``` diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt b/ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt deleted file mode 100644 index 7853e9c..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt +++ /dev/null @@ -1,273 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(roboteq_motor_controller_driver) - -## 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 - roscpp - serial - std_msgs - rospy - tf - message_generation -) - -## 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 - channel_values.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - config_srv.srv - command_srv.srv - maintenance_srv.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 - ) - -################################################ -## 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 roboteq_motor_controller_driver - CATKIN_DEPENDS roscpp serial std_msgs message_runtime -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -include_directories( - include - ${roboteq_motor_controller_driver_INCLUDE_DIRS} - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/roboteq_motor_controller_driver.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/roboteq_motor_controller_driver_node.cpp) -add_executable(roboteq_motor_controller_driver_node - #src/main.cpp - src/roboteq_motor_controller_driver_node.cpp) - #src/configservice.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(roboteq_motor_controller_driver_node roboteq_motor_controller_driver_generate_messages_cpp) - -target_link_libraries(roboteq_motor_controller_driver_node - ${catkin_LIBRARIES} -) - -add_executable(config_client - src/config_client.cpp) - - add_dependencies(config_client roboteq_motor_controller_driver_generate_messages_cpp) - -target_link_libraries(config_client - ${catkin_LIBRARIES} -) - -add_executable(command_client - src/command_client.cpp) - - add_dependencies(command_client roboteq_motor_controller_driver_generate_messages_cpp) - -target_link_libraries(command_client - ${catkin_LIBRARIES} -) - - -add_executable(maintenance_client - src/maintenance_client.cpp) - - add_dependencies(maintenance_client roboteq_motor_controller_driver_generate_messages_cpp) - -target_link_libraries(maintenance_client - ${catkin_LIBRARIES} -) - -add_executable(diff_odom - src/diff_odom.cpp) - - add_dependencies(diff_odom roboteq_motor_controller_driver_generate_messages_cpp) - -target_link_libraries(diff_odom - ${catkin_LIBRARIES} -) - - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) -#target_link_libraries(roboteq_motor_controller_driver -# ${catkin_LIBRARIES} -#) - -#add_executable(roboteq_motor_controller_driver_node src/#roboteq_motor_controller_driver_node.cpp) - - - - - -############# -## 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 -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -install(TARGETS roboteq_motor_controller_driver_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - 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 -# ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) -#catkin_package() - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_roboteq_motor_controller_driver.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/ROS-Driver-Update/roboteq_motor_controller_driver/config/query.yaml b/ROS-Driver-Update/roboteq_motor_controller_driver/config/query.yaml deleted file mode 100644 index b06d110..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/config/query.yaml +++ /dev/null @@ -1,29 +0,0 @@ -port : "/dev/ttyACM0" -baud : 115200 - -frequencyH : 50 #higher frequency (value is in ms) - -queryH: - motor_amps : ?A - motor_command : ?M - hall_count : ?Cb - hall_speed : ?BS - volts : ?V - feedback : ?F - battery_amps : ?BA - power : ?P - fault_flag : ?FF -# motor_amps2 : ?MA -# motor_command2 : ?FM -# hall_count2 : ?FM -# hall_speed2 : ?FS -# volts2 : ?V -# feedback2 : ?rmp -# battery_amps2 : ?PHA -# power2 : ?sns -# fault_flag2 : ?S -# status_flag : ?FS -# firmware_id : ?FID -# encoder_count : ?C -# encoder_speed : ?S - diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/diff_odom.h b/ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/diff_odom.h deleted file mode 100644 index da9d03f..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/diff_odom.h +++ /dev/null @@ -1,72 +0,0 @@ -#include "ros/ros.h" - -class Odometry_calc{ - -public: - Odometry_calc(); - - void spin(); - - -private: - ros::NodeHandle n; - ros::Subscriber sub; - ros::Subscriber l_wheel_sub; - ros::Subscriber r_wheel_sub; - ros::Publisher odom_pub; - - tf::TransformBroadcaster odom_broadcaster; - //Encoder related variables - double encoder_min; - double encoder_max; - - double encoder_low_wrap; - double encoder_high_wrap; - - double prev_lencoder; - double prev_rencoder; - - double lmult; - double rmult; - - double left; - double right; - - double rate; - - ros::Duration t_delta; - - ros::Time t_next; - - ros::Time then; - - - double enc_left ; - - double enc_right; - - double ticks_meter; - - double base_width; - - double dx; - - double dr; - - double x_final,y_final, theta_final; - - ros::Time current_time, last_time; - - - void leftencoderCb(const roboteq_motor_controller_driver::channel_values& left_ticks); - - void rightencoderCb(const roboteq_motor_controller_driver::channel_values& right_ticks); - //void rightencoderCb(std_msgs::Int64::ConstPtr& right_ticks); - void init_variables(); - - void get_node_params(); - - - void update(); -}; - diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/launch/diff_odom.launch b/ROS-Driver-Update/roboteq_motor_controller_driver/launch/diff_odom.launch deleted file mode 100644 index 2163396..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/launch/diff_odom.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/launch/driver.launch b/ROS-Driver-Update/roboteq_motor_controller_driver/launch/driver.launch deleted file mode 100644 index 445c9c1..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/launch/driver.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/msg/channel_values.msg b/ROS-Driver-Update/roboteq_motor_controller_driver/msg/channel_values.msg deleted file mode 100644 index dd2029c..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/msg/channel_values.msg +++ /dev/null @@ -1 +0,0 @@ -int64[] value diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/package.xml b/ROS-Driver-Update/roboteq_motor_controller_driver/package.xml deleted file mode 100644 index 5f25ee8..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/package.xml +++ /dev/null @@ -1,70 +0,0 @@ - - - roboteq_motor_controller_driver - 0.0.0 - The roboteq_motor_controller_driver package - - - - - sushrut - - - - - - TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin - roscpp - roslaunch - serial - std_msgs - message_generation - roscpp - serial - std_msgs - roscpp - serial - std_msgs - message_runtime - - - - - - - diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/command_client.cpp b/ROS-Driver-Update/roboteq_motor_controller_driver/src/command_client.cpp deleted file mode 100644 index 2b1858d..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/command_client.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "ros/ros.h" -#include - -int main(int argc, char **argv) - { - ros::init(argc, argv, "command_client"); - ros::NodeHandle nh; - ros::ServiceClient client = nh.serviceClient("command_service"); - roboteq_motor_controller_driver::command_srv srv; - srv.request.userInput = argv[1]; - srv.request.channel = atoll(argv[2]); - srv.request.value = atoll(argv[3]); - if (client.call(srv)) - { - ROS_INFO("success!"); - } - else - { - ROS_ERROR("Failed to call service"); - return 1; - } - - return 0; - } diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp b/ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp deleted file mode 100644 index 664d2b1..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "ros/ros.h" -#include - -int main(int argc, char **argv) - { - ros::init(argc, argv, "config_client"); - ros::NodeHandle nh; - ros::ServiceClient client = nh.serviceClient("config_service"); - roboteq_motor_controller_driver::config_srv srv; - srv.request.userInput = argv[1]; - srv.request.channel = atoll(argv[2]); - srv.request.value = atoll(argv[3]); - if (client.call(srv)) - { - ROS_INFO("success!"); - } - else - { - ROS_ERROR("Failed to call service"); - return 1; - } - - return 0; - } diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/diff_odom.cpp b/ROS-Driver-Update/roboteq_motor_controller_driver/src/diff_odom.cpp deleted file mode 100644 index 989a824..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/diff_odom.cpp +++ /dev/null @@ -1,374 +0,0 @@ -#include "ros/ros.h" -#include "std_msgs/String.h" -#include "std_msgs/Int64.h" -#include "std_msgs/Int16.h" -#include -#include -#include -#include - - - -class Odometry_calc{ - -public: - Odometry_calc(); - - void spin(); - - -private: - ros::NodeHandle n; - ros::Subscriber sub; - ros::Subscriber l_wheel_sub; - ros::Subscriber r_wheel_sub; - ros::Publisher odom_pub; - - tf::TransformBroadcaster odom_broadcaster; - //Encoder related variables - double encoder_min; - double encoder_max; - - double encoder_low_wrap; - double encoder_high_wrap; - - double prev_lencoder; - double prev_rencoder; - - double lmult; - double rmult; - - double left; - double right; - - double rate; - - ros::Duration t_delta; - - ros::Time t_next; - - ros::Time then; - - - double enc_left ; - - double enc_right; - - double ticks_meter; - - double base_width; - - double dx; - - double dr; - - double x_final,y_final, theta_final; - - ros::Time current_time, last_time; - - - void leftencoderCb(const roboteq_motor_controller_driver::channel_values& left_ticks); - - void rightencoderCb(const roboteq_motor_controller_driver::channel_values& right_ticks); - void init_variables(); - - - - - void update(); -}; - -Odometry_calc::Odometry_calc(){ - - - init_variables(); - - ROS_INFO("Started odometry computing node"); - - l_wheel_sub = n.subscribe("/hall_count",1000, &Odometry_calc::leftencoderCb, this); - - r_wheel_sub = n.subscribe("/hall_count",1000, &Odometry_calc::rightencoderCb, this); - - - odom_pub = n.advertise("odom1", 50); - - - - //Retrieving parameters of this node - -} - -void Odometry_calc::init_variables() -{ - - - prev_lencoder = 0; - prev_rencoder = 0; - - lmult = 0; - rmult = 0; - - left = 0; - right = 0; - - encoder_min = -65536; - encoder_max = 65536; - - rate = 10; - - ticks_meter = 50; - - base_width = 0.3; - - - - - encoder_low_wrap = ((encoder_max - encoder_min) * 0.3) + encoder_min ; - encoder_high_wrap = ((encoder_max - encoder_min) * 0.7) + encoder_min ; - - t_delta = ros::Duration(1.0 / rate); - t_next = ros::Time::now() + t_delta; - - then = ros::Time::now(); - - - enc_left = 10; - enc_right = 0; - - dx = 0; - dr = 0; - - x_final = 0;y_final=0;theta_final=0; - - current_time = ros::Time::now(); - last_time = ros::Time::now(); - -} - - - - - -//Spin function -void Odometry_calc::spin(){ - - ros::Rate loop_rate(rate); - - while (ros::ok()) - { - update(); - loop_rate.sleep(); - - } - - -} - -//Update function -void Odometry_calc::update(){ - - ros::Time now = ros::Time::now(); - -// ros::Time elapsed; - - double elapsed; - - double d_left, d_right, d, th,x,y; - - - if ( now > t_next) { - - elapsed = now.toSec() - then.toSec(); - - // ROS_INFO_STREAM("elapsed =" << elapsed); - - - - if(enc_left == 0){ - d_left = 0; - d_right = 0; - } - else{ - d_left = (left - enc_left) / ( ticks_meter); - d_right = (right - enc_right) / ( ticks_meter); - } - - enc_left = left; - //ROS_INFO_STREAM(left); - enc_right = right; - - d = (d_left + d_right ) / 2.0; - - ROS_INFO_STREAM(d_left << " : " << d_right); - - - th = ( d_right - d_left ) / base_width; - - dx = d /elapsed; - - dr = th / elapsed; - - - if ( d != 0){ - - x = cos( th ) * d; - //ROS_INFO_STREAM(x); - y = -sin( th ) * d; - // calculate the final position of the robot - x_final = x_final + ( cos( theta_final ) * x - sin( theta_final ) * y ); - y_final = y_final + ( sin( theta_final ) * x + cos( theta_final ) * y ); - - } - - if( th != 0) - theta_final = theta_final + th; - - geometry_msgs::Quaternion odom_quat ; - - odom_quat.x = 0.0; - odom_quat.y = 0.0; - odom_quat.z = 0.0; - - odom_quat.z = sin( theta_final / 2 ); - odom_quat.w = cos( theta_final / 2 ); - - //first, we'll publish the transform over tf - geometry_msgs::TransformStamped odom_trans; - odom_trans.header.stamp = now; - odom_trans.header.frame_id = "odom"; - odom_trans.child_frame_id = "base_footprint"; - - odom_trans.transform.translation.x = x_final; - odom_trans.transform.translation.y = y_final; - odom_trans.transform.translation.z = 0.0; - odom_trans.transform.rotation = odom_quat; - - //send the transform - odom_broadcaster.sendTransform(odom_trans); - - - //next, we'll publish the odometry message over ROS - nav_msgs::Odometry odom; - odom.header.stamp = now; - odom.header.frame_id = "odom"; - - //set the position - odom.pose.pose.position.x = x_final; - odom.pose.pose.position.y = y_final; - odom.pose.pose.position.z = 0.0; - odom.pose.pose.orientation = odom_quat; - - //set the velocity - odom.child_frame_id = "base_footprint"; - odom.twist.twist.linear.x = dx; - odom.twist.twist.linear.y = 0; - odom.twist.twist.angular.z = dr; - - //publish the message - odom_pub.publish(odom); - - then = now; - -// ROS_INFO_STREAM("dx =" << x_final); - -// ROS_INFO_STREAM("dy =" << y_final); - - ros::spinOnce(); - - - } - else { ; } -// ROS_INFO_STREAM("Not in loop"); - - - - - -} - - - - - -void Odometry_calc::leftencoderCb(const roboteq_motor_controller_driver::channel_values& left_ticks) - -{ - -// ROS_INFO_STREAM("Left tick" << left_ticks->data); - double enc = left_ticks.value[1]; - - - - if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap)) - { - - lmult = lmult + 1; - } - - - if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap)) - - { - - lmult = lmult - 1; - } - - left = 1.0 * (enc + lmult * (encoder_max - encoder_min )); - - prev_lencoder = enc; - -// ROS_INFO_STREAM("Left " << left); - -} - - -//Right encoder callback - -void Odometry_calc::rightencoderCb(const roboteq_motor_controller_driver::channel_values& right_ticks) - -{ - - -// ROS_INFO_STREAM("Right tick" << right_ticks->data); - - - double enc = right_ticks.value[2]; - - if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap)) - { - - rmult = rmult + 1; - } - - - if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap)) - - { - - rmult = rmult - 1; - } - - right = 1.0 * (enc + rmult * (encoder_max - encoder_min )); - - prev_rencoder = enc; - -// ROS_INFO_STREAM("Right " << right); - - - -} - - - - -int main(int argc, char **argv) - -{ - ros::init(argc, argv,"diff_odom"); - Odometry_calc obj; - obj.spin(); - - - return 0; - -} diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp b/ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp deleted file mode 100644 index 02fe9cb..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp +++ /dev/null @@ -1,35 +0,0 @@ -#include - -bool Driver::configservice(roboteq_motor_controller_driver::config_srv::Request &request, roboteq_motor_controller_driver::config_srv::Response &response) -{ - std::stringstream str; - str << "^" << request.userInput << " " << request.channel << " " << request.value << "_ " << "%\clsav321654987"; - ser.write(str.str()); - response.result = str.str(); - - ROS_INFO_STREAM(response.result); - return true; -} - -bool Driver::commandservice(roboteq_motor_controller_driver::command_srv::Request &request, roboteq_motor_controller_driver::command_srv::Response &response) -{ - std::stringstream str; - str << "%" << request.userInput << " "<< "_"; - ser.write(str.str()); - response.result = str.str(); - - ROS_INFO_STREAM(response.result); - return true; -} - -bool Driver::maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request &request, roboteq_motor_controller_driver::maintenance_srv::Response &response) -{ - std::stringstream str; - str << "%" << request.userInput << " " << "_"; - ser.write(str.str()); - - response.result = ser.read(ser.available()); - - ROS_INFO_STREAM(response.result); - return true; -} diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp b/ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp deleted file mode 100644 index 8967190..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "ros/ros.h" -#include - -int main(int argc, char **argv) - { - ros::init(argc, argv, "maintenance_client"); - ros::NodeHandle nh; - ros::ServiceClient client = nh.serviceClient("maintenance_service"); - roboteq_motor_controller_driver::maintenance_srv srv; - srv.request.userInput = argv[1]; - - - if (client.call(srv)) - { - ROS_INFO("success!"); - } - else - { - ROS_ERROR("Failed to call service"); - return 1; - } - - return 0; - } diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/roboteq_motor_controller_driver_node.cpp b/ROS-Driver-Update/roboteq_motor_controller_driver/src/roboteq_motor_controller_driver_node.cpp deleted file mode 100644 index 147189d..0000000 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/roboteq_motor_controller_driver_node.cpp +++ /dev/null @@ -1,278 +0,0 @@ -// #include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class RoboteqDriver -{ -public: - RoboteqDriver() - { - initialize(); //constructor - Initialize - } - - ~RoboteqDriver() - { - if (ser.isOpen()) - { - ser.close(); - } - } - -private: - serial::Serial ser; - std::string port; - int32_t baud; - ros::Publisher read_publisher; - ros::Subscriber cmd_vel_sub; - - int channel_number_1; - int channel_number_2; - int frequencyH; - int frequencyL; - int frequencyG; - ros::NodeHandle nh; - - void initialize() - { - - nh.getParam("port", port); - nh.getParam("baud", baud); - cmd_vel_sub = nh.subscribe("/cmd_vel", 10, &RoboteqDriver::cmd_vel_callback, this); - - connect(); - } - - void connect() - { - - try - { - - ser.setPort(port); - ser.setBaudrate(baud); //get baud as param - serial::Timeout to = serial::Timeout::simpleTimeout(1000); - ser.setTimeout(to); - ser.open(); - } - catch (serial::IOException &e) - { - - ROS_ERROR_STREAM("Unable to open port "); - ROS_INFO_STREAM("Unable to open port"); - ; - } - if (ser.isOpen()) - { - - ROS_INFO_STREAM("Serial Port initialized\""); - } - else - { - // ROS_INFO_STREAM("HI4"); - ROS_INFO_STREAM("Serial Port is not open"); - } - run(); - } - - void cmd_vel_callback(const geometry_msgs::Twist &msg) - { - std::stringstream cmd_sub; - cmd_sub << "!G 1" - << " " << msg.linear.x << "_" - << "!G 2" - << " " << msg.angular.z << "_"; - - ser.write(cmd_sub.str()); - ser.flush(); - ROS_INFO_STREAM(cmd_sub.str()); - } - - ros::NodeHandle n; - ros::ServiceServer configsrv; - ros::ServiceServer commandsrv; - ros::ServiceServer maintenancesrv; - - bool configservice(roboteq_motor_controller_driver::config_srv::Request &request, roboteq_motor_controller_driver::config_srv::Response &response) - { - std::stringstream str; - str << "^" << request.userInput << " " << request.channel << " " << request.value << "_ " - << "%\clsav321654987"; - ser.write(str.str()); - ser.flush(); - response.result = str.str(); - - ROS_INFO_STREAM(response.result); - return true; - } - - bool commandservice(roboteq_motor_controller_driver::command_srv::Request &request, roboteq_motor_controller_driver::command_srv::Response &response) - { - std::stringstream str; - str << "!" << request.userInput << " " << request.channel << " " << request.value << "_"; - ser.write(str.str()); - ser.flush(); - response.result = str.str(); - - ROS_INFO_STREAM(response.result); - return true; - } - - bool maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request &request, roboteq_motor_controller_driver::maintenance_srv::Response &response) - { - std::stringstream str; - str << "%" << request.userInput << " " - << "_"; - ser.write(str.str()); - ser.flush(); - response.result = ser.read(ser.available()); - - ROS_INFO_STREAM(response.result); - return true; - } - - void initialize_services() - { - n = ros::NodeHandle(); - configsrv = n.advertiseService("config_service", &RoboteqDriver::configservice, this); - commandsrv = n.advertiseService("command_service", &RoboteqDriver::commandservice, this); - maintenancesrv = n.advertiseService("maintenance_service", &RoboteqDriver::maintenanceservice, this); - } - - void run() - { - initialize_services(); - std_msgs::String str1; - ros::NodeHandle nh; - nh.getParam("frequencyH", frequencyH); - nh.getParam("frequencyL", frequencyL); - nh.getParam("frequencyG", frequencyG); - - typedef std::string Key; - typedef std::string Val; - std::map map_sH; - nh.getParam("queryH", map_sH); - - std::stringstream ss0; - std::stringstream ss1; - std::stringstream ss2; - std::stringstream ss3; - std::vector KH_vector; - - ss0 << "^echof 1_"; - ss1 << "# c_/\"DH?\",\"?\""; - for (std::map::iterator iter = map_sH.begin(); iter != map_sH.end(); ++iter) - { - Key KH = iter->first; - - KH_vector.push_back(KH); - - Val VH = iter->second; - - ss1 << VH << "_"; - } - ss1 << "# " << frequencyH << "_"; - - std::vector publisherVecH; - for (int i = 0; i < KH_vector.size(); i++) - { - publisherVecH.push_back(nh.advertise(KH_vector[i], 100)); - } - - ser.write(ss0.str()); - ser.write(ss1.str()); - ser.write(ss2.str()); - ser.write(ss3.str()); - - ser.flush(); - int count = 0; - read_publisher = nh.advertise("read", 1000); - sleep(2); - ros::Rate loop_rate(5); - while (ros::ok()) - { - - ros::spinOnce(); - if (ser.available()) - { - - std_msgs::String result; - result.data = ser.read(ser.available()); - - read_publisher.publish(result); - boost::replace_all(result.data, "\r", ""); - boost::replace_all(result.data, "+", ""); - - std::vector fields; - - std::vector Field9; - boost::split(fields, result.data, boost::algorithm::is_any_of("D")); - - std::vector fields_H; - boost::split(fields_H, fields[1], boost::algorithm::is_any_of("?")); - - if (fields_H[0] == "H") - { - - for (int i = 0; i < publisherVecH.size(); ++i) - { - - std::vector sub_fields_H; - - boost::split(sub_fields_H, fields_H[i + 1], boost::algorithm::is_any_of(":")); - roboteq_motor_controller_driver::channel_values Q1; - - for (int j = 0; j < sub_fields_H.size(); j++) - { - - try - { - Q1.value.push_back(boost::lexical_cast(sub_fields_H[j])); - } - catch (const std::exception &e) - { - count++; - if (count > 10) - { - ROS_INFO_STREAM("Garbage data on Serial"); - //std::cerr << e.what() << '\n'; - } - } - } - - publisherVecH[i].publish(Q1); - } - } - } - loop_rate.sleep(); - } - } -}; - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "roboteq_motor_controller_driver"); - - RoboteqDriver driver; - - ros::waitForShutdown(); - - return 0; -} diff --git a/roboteq_controller/CMakeLists.txt b/roboteq_controller/CMakeLists.txt new file mode 100644 index 0000000..70519ea --- /dev/null +++ b/roboteq_controller/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(roboteq_controller) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(rcutils REQUIRED) +find_package(roboteq_interfaces REQUIRED) +find_package(serial REQUIRED) +find_package(geometry_msgs REQUIRED) + + +include_directories(include) +include_directories(/tmp/usr/local/include/) + +set(node_plugins "") + + + +add_executable(roboteq_controller_node src/roboteq_controller_node.cpp) +ament_target_dependencies(roboteq_controller_node rclcpp std_msgs roboteq_interfaces serial geometry_msgs) + +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() + +install(TARGETS + roboteq_controller_node + DESTINATION lib/${PROJECT_NAME}) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME} +) \ No newline at end of file diff --git a/roboteq_controller/config/query.yaml b/roboteq_controller/config/query.yaml new file mode 100644 index 0000000..66faa04 --- /dev/null +++ b/roboteq_controller/config/query.yaml @@ -0,0 +1,40 @@ +roboteq_controller_node: + ros__parameters: + serial_port: "/dev/ttyACM0" # Udev rule to make a virtual port that's associated to specific hw + baudrate: 115200 + cmd_vel_topic: "/cmd_vel" + + closed_loop: true + + # For differential drive control (dual channel controller) + diff_drive_mode: true + wheel_circumference: 0.6594 # [m] + track_width: .6223 # [m] + max_rpm: 100.0 # [rpm] + encoder_resolution: 1024 + + # For querying data from the motor controller + frequency: 50 #higher frequency (value is in ms) + query: + motor_amps : ?A + motor_command : ?M + # hall_count : ?Cb + # hall_speed : ?BS + volts : ?V + feedback : ?F + # battery_amps : ?BA + # power : ?P + # fault_flag : ?FF + # motor_amps2 : ?MA + # motor_command2 : ?FM + # hall_count2 : ?FM + # hall_speed2 : ?FS + # volts2 : ?V + # feedback2 : ?rmp + # battery_amps2 : ?PHA + # power2 : ?sns + # fault_flag2 : ?S + # status_flag : ?FS + # firmware_id : ?FID + encoder_count : ?C + encoder_speed : ?S diff --git a/roboteq_controller/include/roboteq_controller/diff_odom_node.h b/roboteq_controller/include/roboteq_controller/diff_odom_node.h new file mode 100644 index 0000000..c5ef1a3 --- /dev/null +++ b/roboteq_controller/include/roboteq_controller/diff_odom_node.h @@ -0,0 +1,56 @@ +#pragma once + +#include +#include +#include + +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "std_msgs/Int64.h" +#include "std_msgs/Int16.h" +#include +#include + +#include +#include +#include +#include + + +class RoboteqOdom{ + +public: + RoboteqOdom(ros::NodeHandle, ros::NodeHandle ); + + void spin(); + +private: + ros::NodeHandle nh_, nh_priv_; + ros::Subscriber encoder_sub_; + ros::Publisher odom_pub_; + + tf::TransformBroadcaster odom_broadcaster; + + + int64_t encoder_left_prev_, + encoder_right_prev_, + encoder_left_, + encoder_right_; + + double wheel_circumference_, + track_width_, + encoder_resolution_, + max_rpm_, + max_angular_vel_; + + const int64_t encoder_min_, + encoder_max_; + + ros::Time current_time_, last_time_; + + std::string odom_frame_, child_frame_; + nav_msgs::Odometry odom_; + + std::mutex locker; + void encoderCallback(const roboteq_controller::channel_values& ); +}; \ No newline at end of file diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/roboteq_motor_controller_driver_node.h b/roboteq_controller/include/roboteq_controller/init_service.h similarity index 54% rename from ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/roboteq_motor_controller_driver_node.h rename to roboteq_controller/include/roboteq_controller/init_service.h index efc47c7..d91b71b 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/roboteq_motor_controller_driver_node.h +++ b/roboteq_controller/include/roboteq_controller/init_service.h @@ -1,35 +1,37 @@ #ifndef ROBOTEQ_MOTOR_CONTROLLER_DRIVER_MAIN_H #define ROBOTEQ_MOTOR_CONTROLLER_DRIVER_MAIN_H -#include -#include #include #include //! ROS standard msgs -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// #include +// #include +// #include +// #include +// #include +// #include +// #include +// #include #include #include -#include #include -#include -#include -#include -#include + +// #include +// #include +// #include +// #include + +#include + +#include + +#include +#include +#include + #include -#include +#include #include #include @@ -68,11 +70,11 @@ class Driver void roboteq_services(); - bool configservice(roboteq_motor_controller_driver::config_srv::Request& req, roboteq_motor_controller_driver::config_srv::Response& res); + bool configservice(roboteq_controller::config_srv::Request& req, roboteq_controller::config_srv::Response& res); - bool commandservice(roboteq_motor_controller_driver::command_srv::Request& req, roboteq_motor_controller_driver::command_srv::Response& res); + bool commandservice(roboteq_controller::command_srv::Request& req, roboteq_controller::command_srv::Response& res); - bool maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request& req, roboteq_motor_controller_driver::maintenance_srv::Response& res); + bool maintenanceservice(roboteq_controller::maintenance_srv::Request& req, roboteq_controller::maintenance_srv::Response& res); private: @@ -82,9 +84,9 @@ class Driver int channel; -geometry_msgs::TransformStamped tf_msg; -tf::TransformBroadcaster odom_broadcaster; -nav_msgs::Odometry odom_msg; + geometry_msgs::TransformStamped tf_msg; + tf::TransformBroadcaster odom_broadcaster; + nav_msgs::Odometry odom_msg; nav_msgs::Odometry odom; enum fault_flag diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/querylist.h b/roboteq_controller/include/roboteq_controller/querylist.h similarity index 100% rename from ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/querylist.h rename to roboteq_controller/include/roboteq_controller/querylist.h diff --git a/roboteq_controller/include/roboteq_controller/roboteq_controller_node.h b/roboteq_controller/include/roboteq_controller/roboteq_controller_node.h new file mode 100644 index 0000000..c503445 --- /dev/null +++ b/roboteq_controller/include/roboteq_controller/roboteq_controller_node.h @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include +#include + + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/empty.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "roboteq_interfaces/msg/channel_values.hpp" + +// #include "roboteq_controller/msg/config.hpp" +// #include "roboteq_controller/msg/maintenance.hpp" + +// #include "roboteq_controller/querylist.h" +// #include "roboteq_controller/channel_values.h" + + +using namespace std::chrono_literals; +using std::placeholders::_1; + +class RoboteqDriver : public rclcpp::Node +{ +public: + explicit RoboteqDriver(const rclcpp::NodeOptions &options = rclcpp::NodeOptions()); + + ~RoboteqDriver(){ + if (ser_.isOpen()){ + ser_.close(); + } + } + +private: + + // Serial + serial::Serial ser_; + std::string serial_port_; + int32_t baudrate_; + + // Pub & Sub + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Publisher::SharedPtr serial_read_pub_; + + std::vector::SharedPtr> query_pub_; + + rclcpp::TimerBase::SharedPtr timer_pub_; + + bool closed_loop_, + diff_drive_mode_; + + double wheel_circumference_, + track_width_, + max_rpm_; + + std::string cmd_vel_topic_; + + // queries + std::map queries_; + + // int channel_number_1; + // int channel_number_2; + int frequency_; + std::mutex locker; + + void declare(); + void init(); + + void cmdSetup(); + void cmdVelCallback(const geometry_msgs::msg::Twist &); + void powerCmdCallback(const geometry_msgs::msg::Twist &); + // bool configService(roboteq_interfaces::config_srv::Request &, roboteq_interfaces::config_srv::Response &); + // bool commandService(roboteq_interfaces::command_srv::Request &, roboteq_interfaces::command_srv::Response &); + // bool maintenanceService(roboteq_interfaces::maintenance_srv::Request &, roboteq_interfaces::maintenance_srv::Response &); + void initializeServices(); + void run(); + + void queryCallback(); + +}; \ No newline at end of file diff --git a/roboteq_controller/launch/roboteq_launch.py b/roboteq_controller/launch/roboteq_launch.py new file mode 100644 index 0000000..845d13d --- /dev/null +++ b/roboteq_controller/launch/roboteq_launch.py @@ -0,0 +1,21 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + ld = LaunchDescription() + config = os.path.join( + get_package_share_directory('roboteq_controller'), + 'config', + 'query.yaml' + ) + + node=Node( + package = 'roboteq_controller', + name = 'roboteq_controller_node', + executable = 'roboteq_controller_node', + parameters = [config] + ) + ld.add_action(node) + return ld \ No newline at end of file diff --git a/roboteq_controller/package.xml b/roboteq_controller/package.xml new file mode 100644 index 0000000..24a1bfd --- /dev/null +++ b/roboteq_controller/package.xml @@ -0,0 +1,27 @@ + + + roboteq_controller + 0.0.5 + The roboteq_controller package + Doan Nguyen + + MIT + + ament_cmake + rclcpp + std_msgs + + roboteq_interfaces + serial + geometry_msgs + + ros2launch + ament_lint_auto + ament_lint_common + + + + + ament_cmake + + diff --git a/roboteq_controller/src/roboteq_controller_node.cpp b/roboteq_controller/src/roboteq_controller_node.cpp new file mode 100644 index 0000000..3a74cf8 --- /dev/null +++ b/roboteq_controller/src/roboteq_controller_node.cpp @@ -0,0 +1,384 @@ +#include "roboteq_controller/roboteq_controller_node.h" + +// static const std::string tag {"[RoboteQ] "}; +static const std::string tag {""}; + +void RoboteqDriver::declare(){ + declare_parameter("serial_port", "dev/ttyUSB0"); + declare_parameter("baudrate", 112500); + declare_parameter("closed_loop", false); + declare_parameter("diff_drive_mode", false); + declare_parameter("wheel_circumference", 0.0); + declare_parameter("track_width", 0.0); + declare_parameter("max_rpm", 0.0); + declare_parameter("frequency", 0); + declare_parameter("cmd_vel_topic", "cmd_vel"); +} + +void RoboteqDriver::init(){ + RCLCPP_INFO(get_logger(), "Creating"); + get_parameter("frequency", frequency_); + + get_parameter("serial_port", serial_port_); + get_parameter("baudrate", baudrate_); + + get_parameter("closed_loop", closed_loop_); + get_parameter("diff_drive_mode", diff_drive_mode_); + + get_parameter("wheel_circumference", wheel_circumference_); + get_parameter("track_width", track_width_); + get_parameter("max_rpm", max_rpm_); + get_parameter("cmd_vel_topic", cmd_vel_topic_); + + RCLCPP_INFO_STREAM(this->get_logger(),tag << "cmd_vel:" << cmd_vel_topic_); + + if (closed_loop_){ + RCLCPP_INFO_STREAM(this->get_logger(),tag << "In CLOSED-LOOP mode!!!! serial port:" << serial_port_); + } + else{ + RCLCPP_INFO_STREAM(this->get_logger(),tag << "In OPEN-LOOP mode!!!!"); + } + + if (wheel_circumference_ <=0.0 ){ + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Inproper configuration! wheel_circumference need to be greater than zero."); + } + if (track_width_ <=0.0 ){ + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Inproper configuration! track_width need to be greater than zero."); + } + if ( max_rpm_ <=0.0 ){ + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Inproper configuration! max_rpm need to be greater than zero."); + } + + if (frequency_ <= 0.0){ + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Inproper configuration! \'frequency\' need to be greater than zero."); + } + + // Nested params for queries + // get_parameter(); + + auto param_interface = this->get_node_parameters_interface(); + std::map params = param_interface->get_parameter_overrides(); + + RCLCPP_INFO_STREAM(this->get_logger(), tag << "queries:" ); + + for (auto iter = params.begin(); iter != params.end(); iter++){ + std::size_t pos = iter->first.find("query"); + if (pos != std::string::npos && + iter->second.get_type() == rclcpp::ParameterType::PARAMETER_STRING){ + std::string topic = iter->first.substr (pos+ 6); + auto query = iter->second.to_value_msg().string_value; + + queries_[topic] = query; + RCLCPP_INFO(this->get_logger(), "%15s : %s", topic.c_str(), query.c_str() ); + } + } +} + + +RoboteqDriver::RoboteqDriver(const rclcpp::NodeOptions &options): Node("roboteq_controller", options), + wheel_circumference_(0.), + track_width_(0.), + max_rpm_(0.), + frequency_(0), + serial_port_("dev/ttyUSB0"), + baudrate_(112500), + closed_loop_(false), + diff_drive_mode_(false), + cmd_vel_topic_("cmd_vel"){ + + declare(); + init(); + + if (diff_drive_mode_){ + cmd_vel_sub_ = create_subscription( + cmd_vel_topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&RoboteqDriver::cmdVelCallback, this, std::placeholders::_1)); + } + else{ + cmd_vel_sub_ = create_subscription( + cmd_vel_topic_, rclcpp::SystemDefaultsQoS(), + std::bind(&RoboteqDriver::powerCmdCallback, this, std::placeholders::_1)); + } + + // Initiate communication to serial port + try{ + ser_.setPort(serial_port_); + ser_.setBaudrate(baudrate_); + serial::Timeout timeout = serial::Timeout::simpleTimeout(1000); + ser_.setTimeout(timeout); + ser_.open(); + } + catch (serial::IOException &e){ + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Unable to open port " << serial_port_); + rclcpp::shutdown(); + } + + if (ser_.isOpen()){ + RCLCPP_INFO_STREAM(this->get_logger(),tag << "Serial Port " << serial_port_ << " initialized"); + } + else{ + RCLCPP_INFO_STREAM(this->get_logger(),tag << "Serial Port " << serial_port_ << " is not open"); + rclcpp::shutdown(); + } + + cmdSetup(); + + run(); +} + + +void RoboteqDriver::cmdSetup(){ + // stop motors + ser_.write("!G 1 0\r"); + ser_.write("!G 2 0\r"); + ser_.write("!S 1 0\r"); + ser_.write("!S 2 0\r"); + ser_.flush(); + + // // disable echo + // ser.write("^ECHOF 1\r"); + // ser.flush(); + + // enable watchdog timer (1000 ms) to stop if no connection + ser_.write("^RWD 1000\r"); + + // set motor operating mode (1 for closed-loop speed) + if (!closed_loop_){ + // open-loop speed mode + ser_.write("^MMOD 1 0\r"); + ser_.write("^MMOD 2 0\r"); + ser_.flush(); + } + else{ + // closed-loop speed mode + ser_.write("^MMOD 1 1\r"); + ser_.write("^MMOD 2 1\r"); + ser_.flush(); + } + + // set encoder counts (ppr) + std::stringstream right_enccmd; + std::stringstream left_enccmd; + right_enccmd << "^EPPR 1 " << 1024 << "\r"; + left_enccmd << "^EPPR 2 " << 1024 << "\r"; + ser_.write(right_enccmd.str()); + ser_.write(left_enccmd.str()); + ser_.flush(); +} + + +void RoboteqDriver::run(){ + // initializeServices(); + std::stringstream ss0, ss1; + ss0 << "^echof 1_"; + ss1 << "# c_/\"DH?\",\"?\""; + + for (auto item : queries_){ + RCLCPP_INFO_STREAM(this->get_logger(),tag << "Publish topic: " << item.first); + query_pub_.push_back(create_publisher(item.first, 100)); + + std::string cmd = item.second; + ss1 << cmd << "_"; + } + + ss1 << "# " << frequency_ << "_"; + + ser_.write(ss0.str()); + ser_.write(ss1.str()); + ser_.flush(); + + + serial_read_pub_ = create_publisher("read", rclcpp::SystemDefaultsQoS()); + + std::chrono::duration dt (1000/frequency_); + timer_pub_ = create_wall_timer(dt, std::bind(&RoboteqDriver::queryCallback, this) ); +} + + +void RoboteqDriver::powerCmdCallback(const geometry_msgs::msg::Twist &msg){ + std::stringstream cmd_str; + if (closed_loop_){ + cmd_str << "!S 1" + << " " << msg.linear.x << "_" + << "!S 2" + << " " << msg.angular.z << "_"; + } + else{ + cmd_str << "!G 1" + << " " << msg.linear.x << "_" + << "!G 2" + << " " << msg.angular.z << "_"; + } + ser_.write(cmd_str.str()); + ser_.flush(); + RCLCPP_INFO(this->get_logger(),"[ROBOTEQ] left: %9.3f right: %9.3f", msg.linear.x, msg.angular.z); + // RCLCPP_INFO_STREAM(this->get_logger(),cmd_str.str()); +} + + +void RoboteqDriver::cmdVelCallback(const geometry_msgs::msg::Twist &msg){ + // wheel speed (m/s) + float right_speed = msg.linear.x + track_width_ * msg.angular.z / 2.0; + float left_speed = msg.linear.x - track_width_ * msg.angular.z / 2.0; + + // RCLCPP_INFO(this->get_logger(),("[ROBOTEQ] left: %.3f right: %.3f", left_speed, right_speed); + std::stringstream cmd_str; + if (!closed_loop_){ + // motor power (scale 0-1000) + float right_power = right_speed *1000.0 *60.0/ (wheel_circumference_ * max_rpm_); + float left_power = left_speed *1000.0 *60.0/ (wheel_circumference_ * max_rpm_); + + RCLCPP_INFO(this->get_logger(),"[ROBOTEQ] left: %9d right: %9d", (int)left_power, (int)right_power); + + cmd_str << "!G 1" + << " " << (int)left_power << "_" + << "!G 2" + << " " << (int)right_power << "_"; + } + else{ + // motor speed (rpm) + int32_t right_rpm = right_speed *60.0 / wheel_circumference_; + int32_t left_rpm = left_speed *60.0 / wheel_circumference_; + + RCLCPP_INFO(this->get_logger(),"[ROBOTEQ] left: %9d right: %9d", left_rpm, right_rpm); + cmd_str << "!S 1" + << " " << left_rpm << "_" + << "!S 2" + << " " << right_rpm << "_"; + } + + ser_.write(cmd_str.str()); + ser_.flush(); + // RCLCPP_INFO_STREAM(this->get_logger(),cmd_str.str()); +} + + +// bool RoboteqDriver::configService(roboteq_controller::config_srv::Request &request, +// roboteq_controller::config_srv::Response &response){ +// std::stringstream str; +// str << "^" << request.userInput << " " << request.channel << " " << request.value << "_ " +// << "%\clsav321654987"; +// ser_.write(str.str()); +// ser_.flush(); +// response.result = str.str(); + +// RCLCPP_INFO_STREAM(this->get_logger(),tag << response.result); +// return true; +// } + + +// bool RoboteqDriver::commandService(roboteq_controller::command_srv::Request &request, roboteq_controller::command_srv::Response &response) +// { +// std::stringstream str; +// str << "!" << request.userInput << " " << request.channel << " " << request.value << "_"; +// ser_.write(str.str()); +// ser_.flush(); +// response.result = str.str(); + +// RCLCPP_INFO_STREAM(this->get_logger(),tag << response.result); +// return true; +// } + + +// bool RoboteqDriver::maintenanceService(roboteq_controller::maintenance_srv::Request &request, roboteq_controller::maintenance_srv::Response &response) +// { +// std::stringstream str; +// str << "%" << request.userInput << " " +// << "_"; +// ser_.write(str.str()); +// ser_.flush(); +// response.result = ser_.read(ser_.available()); + +// RCLCPP_INFO_STREAM(this->get_logger(),response.result); +// return true; +// } + + +// void RoboteqDriver::initializeServices(){ +// configsrv_ = nh_.advertiseService("config_service", &RoboteqDriver::configService, this); +// commandsrv_ = nh_.advertiseService("command_service", &RoboteqDriver::commandService, this); +// maintenancesrv_ = nh_.advertiseService("maintenance_service", &RoboteqDriver::maintenanceService, this); +// } + + +void RoboteqDriver::queryCallback(){ + auto current_time = this->now(); + if (ser_.available()){ + std_msgs::msg::String result; + + std::lock_guard lock(locker); + + result.data = ser_.read(ser_.available()); + + // std::lock_guard unlock(locker); + + + serial_read_pub_->publish(result); + + boost::replace_all(result.data, "\r", ""); + boost::replace_all(result.data, "+", ""); + + std::vector fields; + + boost::split(fields, result.data, boost::algorithm::is_any_of("D")); + if (fields.size() < 2){ + + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Empty data:{" << result.data << "}"); + } + else if (fields.size() >= 2){ + std::vector fields_H; + for (int i = fields.size() - 1; i >= 0; i--){ + if (fields[i][0] == 'H'){ + try{ + fields_H.clear(); + boost::split(fields_H, fields[i], boost::algorithm::is_any_of("?")); + if ( fields_H.size() >= query_pub_.size() + 1){ + break; + } + } + catch (const std::exception &e){ + std::cerr << e.what() << '\n'; + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Finding query output in :" << fields[i]); + continue; + } + } + } + + if (fields_H.size() > 0 && fields_H[0] == "H"){ + for (int i = 0; i < fields_H.size() - 1; ++i){ + std::vector sub_fields_H; + boost::split(sub_fields_H, fields_H[i + 1], boost::algorithm::is_any_of(":")); + + roboteq_interfaces::msg::ChannelValues msg; + msg.header.stamp = current_time; + + for (int j = 0; j < sub_fields_H.size(); j++){ + try{ + msg.value.push_back(boost::lexical_cast(sub_fields_H[j])); + } + catch (const std::exception &e){ + RCLCPP_ERROR_STREAM(this->get_logger(),tag << "Garbage data on Serial"); + RCLCPP_ERROR_STREAM(this->get_logger(), result.data); + RCLCPP_ERROR_STREAM(this->get_logger(), e.what()); + std::cerr << e.what() << '\n'; + } + } + query_pub_[i]->publish(msg); + } + } + } + else{ + RCLCPP_WARN_STREAM(this->get_logger(),tag << "Unknown:{" << result.data << "}"); + } + } +} + + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} + diff --git a/roboteq_interfaces/CMakeLists.txt b/roboteq_interfaces/CMakeLists.txt new file mode 100644 index 0000000..48f32f4 --- /dev/null +++ b/roboteq_interfaces/CMakeLists.txt @@ -0,0 +1,36 @@ +cmake_minimum_required(VERSION 3.8) +project(roboteq_interfaces) + +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) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(std_msgs REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ChannelValues.msg" + "srv/Command.srv" + "srv/Config.srv" + "srv/Maintenance.srv" + DEPENDENCIES builtin_interfaces std_msgs +) + +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/roboteq_interfaces/msg/ChannelValues.msg b/roboteq_interfaces/msg/ChannelValues.msg new file mode 100644 index 0000000..8280bc0 --- /dev/null +++ b/roboteq_interfaces/msg/ChannelValues.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +int64[] value \ No newline at end of file diff --git a/roboteq_interfaces/package.xml b/roboteq_interfaces/package.xml new file mode 100644 index 0000000..076e252 --- /dev/null +++ b/roboteq_interfaces/package.xml @@ -0,0 +1,23 @@ + + + + roboteq_interfaces + 0.0.0 + TODO: Package description + parallels + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/command_srv.srv b/roboteq_interfaces/srv/Command.srv similarity index 70% rename from ROS-Driver-Update/roboteq_motor_controller_driver/srv/command_srv.srv rename to roboteq_interfaces/srv/Command.srv index 84b7eca..74a946e 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/command_srv.srv +++ b/roboteq_interfaces/srv/Command.srv @@ -1,4 +1,4 @@ -string userInput +string user_input int64 channel int64 value --- diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/config_srv.srv b/roboteq_interfaces/srv/Config.srv similarity index 70% rename from ROS-Driver-Update/roboteq_motor_controller_driver/srv/config_srv.srv rename to roboteq_interfaces/srv/Config.srv index 84b7eca..74a946e 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/config_srv.srv +++ b/roboteq_interfaces/srv/Config.srv @@ -1,4 +1,4 @@ -string userInput +string user_input int64 channel int64 value --- diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/maintenance_srv.srv b/roboteq_interfaces/srv/Maintenance.srv similarity index 50% rename from ROS-Driver-Update/roboteq_motor_controller_driver/srv/maintenance_srv.srv rename to roboteq_interfaces/srv/Maintenance.srv index cb8c65b..eaae4c4 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/maintenance_srv.srv +++ b/roboteq_interfaces/srv/Maintenance.srv @@ -1,3 +1,3 @@ -string userInput +string user_input --- string result