└── ROS-Driver-Update ├── LICENSE ├── README.md └── roboteq_motor_controller_driver ├── CMakeLists.txt ├── config └── query.yaml ├── include └── roboteq_motor_controller_driver │ ├── diff_odom.h │ ├── querylist.h │ └── roboteq_motor_controller_driver_node.h ├── launch ├── diff_odom.launch └── driver.launch ├── msg └── channel_values.msg ├── package.xml ├── src ├── command_client.cpp ├── config_client.cpp ├── diff_odom.cpp ├── init_service.cpp ├── maintenance_client.cpp └── roboteq_motor_controller_driver_node.cpp └── srv ├── command_srv.srv ├── config_srv.srv └── maintenance_srv.srv /ROS-Driver-Update/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 Roboteq-Inc 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /ROS-Driver-Update/README.md: -------------------------------------------------------------------------------- 1 | # ROS-Driver 2 | # Quick Start Guide 3 | ``` 4 | Note* 5 | -This ROS driver only supports firmware version 2.0 or 2.0+. 6 | -You can check your firmware version from Roborun+ console tab by querying - "?fid". 7 | -If firmware is not the latest one then please update it with the latest one available on Roboteq website 8 | or contact "techsupport.roboteq@mail.nidec.com". 9 | ``` 10 | 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). 11 | 12 | First, clone this repository to catkin_ws/src 13 | ``` 14 | git clone https://github.com/Roboteq-Inc/ROS-Driver.git 15 | ``` 16 | 17 | 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 - 18 | 19 | ``` 20 | cd catkin_ws/ 21 | source devel/setup.bash 22 | roslaunch roboteq_motor_controller_driver driver.launch 23 | ``` 24 | 25 | 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 26 | 27 | ``` 28 | frequencyH : 50 #higher frequency (value is in ms) 29 | frequencyL : 100 #lower frequency 30 | frequencyG : 100 #frequency for general queries 31 | 32 | queryH: 33 | motor_amps : ?A 34 | motor_command : ?M 35 | encoder_count : ?C 36 | encoder_speed : ?S #these queries will publish with higher frequency and users can add other queries below encoder_speed. 37 | 38 | queryL: 39 | error : ?E 40 | feedback : ?F 41 | battery_amps : ?BA 42 | power : ?P #these queries will publish with lower frequency and users can add other queries below power. 43 | 44 | 45 | queryG: 46 | fault_flag : ?FF 47 | # status_flag : ?FS 48 | # firmware_id : ?FID Users can add queries which do not require channel number under queryG tab. 49 | ``` 50 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roboteq_motor_controller_driver) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | serial 13 | std_msgs 14 | rospy 15 | tf 16 | message_generation 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | # catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | add_message_files( 54 | FILES 55 | channel_values.msg 56 | ) 57 | 58 | ## Generate services in the 'srv' folder 59 | add_service_files( 60 | FILES 61 | config_srv.srv 62 | command_srv.srv 63 | maintenance_srv.srv 64 | # Service2.srv 65 | ) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | generate_messages( 76 | DEPENDENCIES 77 | std_msgs 78 | ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | INCLUDE_DIRS include 111 | # LIBRARIES roboteq_motor_controller_driver 112 | CATKIN_DEPENDS roscpp serial std_msgs message_runtime 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | include 124 | ${roboteq_motor_controller_driver_INCLUDE_DIRS} 125 | ${Boost_INCLUDE_DIRS} 126 | ${catkin_INCLUDE_DIRS} 127 | ) 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/roboteq_motor_controller_driver.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/roboteq_motor_controller_driver_node.cpp) 142 | add_executable(roboteq_motor_controller_driver_node 143 | #src/main.cpp 144 | src/roboteq_motor_controller_driver_node.cpp) 145 | #src/configservice.cpp) 146 | 147 | ## Rename C++ executable without prefix 148 | ## The above recommended prefix causes long target names, the following renames the 149 | ## target back to the shorter version for ease of user use 150 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 151 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | add_dependencies(roboteq_motor_controller_driver_node roboteq_motor_controller_driver_generate_messages_cpp) 156 | 157 | target_link_libraries(roboteq_motor_controller_driver_node 158 | ${catkin_LIBRARIES} 159 | ) 160 | 161 | add_executable(config_client 162 | src/config_client.cpp) 163 | 164 | add_dependencies(config_client roboteq_motor_controller_driver_generate_messages_cpp) 165 | 166 | target_link_libraries(config_client 167 | ${catkin_LIBRARIES} 168 | ) 169 | 170 | add_executable(command_client 171 | src/command_client.cpp) 172 | 173 | add_dependencies(command_client roboteq_motor_controller_driver_generate_messages_cpp) 174 | 175 | target_link_libraries(command_client 176 | ${catkin_LIBRARIES} 177 | ) 178 | 179 | 180 | add_executable(maintenance_client 181 | src/maintenance_client.cpp) 182 | 183 | add_dependencies(maintenance_client roboteq_motor_controller_driver_generate_messages_cpp) 184 | 185 | target_link_libraries(maintenance_client 186 | ${catkin_LIBRARIES} 187 | ) 188 | 189 | add_executable(diff_odom 190 | src/diff_odom.cpp) 191 | 192 | add_dependencies(diff_odom roboteq_motor_controller_driver_generate_messages_cpp) 193 | 194 | target_link_libraries(diff_odom 195 | ${catkin_LIBRARIES} 196 | ) 197 | 198 | 199 | ## Specify libraries to link a library or executable target against 200 | # target_link_libraries(${PROJECT_NAME}_node 201 | # ${catkin_LIBRARIES} 202 | # ) 203 | #target_link_libraries(roboteq_motor_controller_driver 204 | # ${catkin_LIBRARIES} 205 | #) 206 | 207 | #add_executable(roboteq_motor_controller_driver_node src/#roboteq_motor_controller_driver_node.cpp) 208 | 209 | 210 | 211 | 212 | 213 | ############# 214 | ## Install ## 215 | ############# 216 | 217 | # all install targets should use catkin DESTINATION variables 218 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 219 | 220 | ## Mark executable scripts (Python etc.) for installation 221 | ## in contrast to setup.py, you can choose the destination 222 | # install(PROGRAMS 223 | # scripts/my_python_script 224 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 225 | # ) 226 | install(DIRECTORY include/${PROJECT_NAME}/ 227 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 228 | ) 229 | ## Mark executables for installation 230 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 231 | install(TARGETS roboteq_motor_controller_driver_node 232 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 233 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 234 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 235 | ) 236 | 237 | ## Mark libraries for installation 238 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 239 | # install(TARGETS ${PROJECT_NAME} 240 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 241 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 242 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 243 | # ) 244 | 245 | ## Mark cpp header files for installation 246 | # install(DIRECTORY include/${PROJECT_NAME}/ 247 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 248 | # FILES_MATCHING PATTERN "*.h" 249 | # PATTERN ".svn" EXCLUDE 250 | # ) 251 | install(DIRECTORY launch 252 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 253 | ) 254 | ## Mark other files for installation (e.g. launch and bag files, etc.) 255 | # install(FILES 256 | # # myfile1 257 | # # myfile2 258 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 259 | # ) 260 | #catkin_package() 261 | 262 | ############# 263 | ## Testing ## 264 | ############# 265 | 266 | ## Add gtest based cpp test target and link libraries 267 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_roboteq_motor_controller_driver.cpp) 268 | # if(TARGET ${PROJECT_NAME}-test) 269 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 270 | # endif() 271 | 272 | ## Add folders to be run by python nosetests 273 | # catkin_add_nosetests(test) 274 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/config/query.yaml: -------------------------------------------------------------------------------- 1 | port : "/dev/ttyACM0" 2 | baud : 115200 3 | 4 | frequencyH : 50 #higher frequency (value is in ms) 5 | 6 | queryH: 7 | motor_amps : ?A 8 | motor_command : ?M 9 | hall_count : ?Cb 10 | hall_speed : ?BS 11 | volts : ?V 12 | feedback : ?F 13 | battery_amps : ?BA 14 | power : ?P 15 | fault_flag : ?FF 16 | # motor_amps2 : ?MA 17 | # motor_command2 : ?FM 18 | # hall_count2 : ?FM 19 | # hall_speed2 : ?FS 20 | # volts2 : ?V 21 | # feedback2 : ?rmp 22 | # battery_amps2 : ?PHA 23 | # power2 : ?sns 24 | # fault_flag2 : ?S 25 | # status_flag : ?FS 26 | # firmware_id : ?FID 27 | # encoder_count : ?C 28 | # encoder_speed : ?S 29 | 30 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/diff_odom.h: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | 3 | class Odometry_calc{ 4 | 5 | public: 6 | Odometry_calc(); 7 | 8 | void spin(); 9 | 10 | 11 | private: 12 | ros::NodeHandle n; 13 | ros::Subscriber sub; 14 | ros::Subscriber l_wheel_sub; 15 | ros::Subscriber r_wheel_sub; 16 | ros::Publisher odom_pub; 17 | 18 | tf::TransformBroadcaster odom_broadcaster; 19 | //Encoder related variables 20 | double encoder_min; 21 | double encoder_max; 22 | 23 | double encoder_low_wrap; 24 | double encoder_high_wrap; 25 | 26 | double prev_lencoder; 27 | double prev_rencoder; 28 | 29 | double lmult; 30 | double rmult; 31 | 32 | double left; 33 | double right; 34 | 35 | double rate; 36 | 37 | ros::Duration t_delta; 38 | 39 | ros::Time t_next; 40 | 41 | ros::Time then; 42 | 43 | 44 | double enc_left ; 45 | 46 | double enc_right; 47 | 48 | double ticks_meter; 49 | 50 | double base_width; 51 | 52 | double dx; 53 | 54 | double dr; 55 | 56 | double x_final,y_final, theta_final; 57 | 58 | ros::Time current_time, last_time; 59 | 60 | 61 | void leftencoderCb(const roboteq_motor_controller_driver::channel_values& left_ticks); 62 | 63 | void rightencoderCb(const roboteq_motor_controller_driver::channel_values& right_ticks); 64 | //void rightencoderCb(std_msgs::Int64::ConstPtr& right_ticks); 65 | void init_variables(); 66 | 67 | void get_node_params(); 68 | 69 | 70 | void update(); 71 | }; 72 | 73 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/querylist.h: -------------------------------------------------------------------------------- 1 | #ifndef __querylist__ 2 | #define __querylist__ 3 | #include 4 | //////////////////////////// RUNTIME COMMANDS //////////////////////////// 5 | /*#define _G 0 6 | #define _GO 0 7 | #define _M 1 8 | #define _MOTCMD 1 9 | #define _P 2 10 | #define _MOTPOS 2 11 | #define _S 3 12 | #define _MOTVEL 3 13 | #define _C 4 14 | #define _SENCNTR 4 15 | #define _CB 5 16 | #define _VAR 6 17 | #define _SBLCNTR 5 18 | #define _AC 7 19 | #define _ACCEL 7 20 | #define _DC 8 21 | #define _DECEL 8 22 | #define _DS 9 23 | #define _DOUT 9 24 | #define _D1 10 25 | #define _DSET 10 26 | #define _D0 11 27 | #define _DRES 11 28 | #define _R 12 29 | #define _H 13 30 | #define _HOME 13 31 | #define _EX 14 32 | #define _ESTOP 14 33 | #define _MG 15 34 | #define _MGO 15 35 | #define _MS 16 36 | #define _MSTOP 16 37 | #define _PR 17 38 | #define _MPOSREL 17 39 | #define _PX 18 40 | #define _NXTPOS 18 41 | #define _PRX 19 42 | #define _NXTPOSR 19 43 | #define _AX 20 44 | #define _NXTACC 20 45 | #define _DX 21 46 | #define _NXTDEC 21 47 | #define _B 22 48 | #define _BOOL 22 49 | #define _SX 23 50 | #define _NXTVEL 23 51 | #define _CS 24 52 | #define _CANSEND 24 53 | #define _CG 25 54 | #define _CANGO 25 55 | #define _RC 26 56 | #define _RCOUT 26 57 | #define _EES 27 58 | #define _EESAV 27 59 | #define _BND 28 60 | #define _BIND 28 61 | #define _AO 29 62 | #define _AOUT 29 63 | #define _TX 30 64 | #define _LEFT 30 65 | #define _TV 31 66 | #define _RIGHT 31 67 | #define _CSW 32 68 | #define _CELLSW 32 69 | #define _PSW 33 70 | #define _BMSPSW 33 71 | #define _ASW 34 72 | #define _BMSASW 34 73 | #define _HS 49 74 | #define _ANG 65 75 | #define _ANGLE 65 76 | #define _CW 86 77 | #define _SWS 81 78 | #define _SPL 87 79 | #define _SAC 88 80 | #define _SDC 89 81 | #define _ROM 90 82 | #define _TC 91 83 | #define _POS 92 84 | #define _PSP 93 85 | #define _PAC 94 86 | #define _PDC 95 87 | #define _TSL 96 88 | #define _CSS 108 89 | #define _STT 112 90 | #define _PXL 117 91 | #define _ZER 121 92 | #define _MM 120 93 | #define _GIQ 122 94 | #define _GID 123 95 | #define _SCO 124 96 | #define _VPM 125 97 | #define _MKM 131 98 | #define _MKO 132 99 | #define _MKP 133 100 | #define _MKD 134 101 | #define _MKT 135 102 | */ 103 | //////////////////////////// RUNTIME QUERIES //////////////////////////// 104 | //std::string ?A; 105 | #define motor_amps "?A" 106 | #define Motor_Command_Applied "?M" 107 | #define motor_power ?P 108 | #define motor_speed ?S 109 | 110 | /*#define ?A 0 111 | #define ?M 1 112 | #define ?P 2 113 | #define ?S 3 114 | #define _C 4 115 | #define _ABCNTR 4 116 | #define _CB 5 117 | #define _BLCNTR 5 118 | #define _VAR 6 119 | #define _SR 7 120 | #define _RELSPEED 7 121 | #define _CR 8 122 | #define _RELCNTR 8 123 | #define _BCR 9 124 | #define _BLRCNTR 9 125 | #define _BS 10 126 | #define _BLSPEED 10 127 | #define _BSR 11 128 | #define _BLRSPEED 11 129 | #define _BA 12 130 | #define _BATAMPS 12 131 | #define _V 13 132 | #define _VOLTS 13 133 | #define _D 14 134 | #define _DIGIN 14 135 | //#define _DI 15 136 | #define _DIN 15 137 | #define _AI 16 138 | #define _ANAIN 16 139 | #define _PI 17 140 | #define _PLSIN 17 141 | #define _T 18 142 | #define _TEMP 18 143 | #define _F 19 144 | #define _FEEDBK 19 145 | #define _FS 20 146 | #define _STFLAG 20 147 | #define _FF 21 148 | #define _FLTFLAG 21 149 | #define _B 22 150 | #define _BOOL 22 151 | #define _DO 23 152 | #define _DIGOUT 23 153 | #define _E 24 154 | #define _LPERR 24 155 | #define _CIS 25 156 | #define _CMDSER 25 157 | #define _CIA 26 158 | #define _CMDANA 26 159 | #define _CIP 27 160 | #define _CMDPLS 27 161 | #define _TM 28 162 | #define _TIME 28 163 | #define _LK 29 164 | #define _LOCKED 29 165 | #define _FID 30 166 | #define _TRN 31 167 | #define _TR 32 168 | #define _TRACK 32 169 | #define _K 33 170 | #define _SPEKTRUM 33 171 | #define _DR 34 172 | #define _DREACHED 34 173 | #define _AIC 35 174 | #define _ANAINC 35 175 | #define _PIC 36 176 | #define _PLSINC 36 177 | #define _MA 37 178 | #define _MEMS 37 179 | #define _CL 38 180 | #define _CALIVE 38 181 | #define _CAN 39 182 | #define _CF 40 183 | #define _MGD 41 184 | #define _MGDET 41 185 | #define _MGT 42 186 | #define _MGTRACK 42 187 | #define _MGM 43 188 | #define _MGMRKR 43 189 | #define _MGS 44 190 | #define _MGSTATUS 44 191 | #define _MGY 45 192 | #define _MGYRO 45 193 | #define _MBV 46 194 | #define _MBB 47 195 | #define _FM 48 196 | #define _MOTFLAG 48 197 | #define _HS 49 198 | #define _HSENSE 49 199 | #define _UID 50 200 | #define _ASI 51 201 | #define _RAWSENSADC 51 202 | #define _QO 52 203 | #define _QOUT 52 204 | #define _EO 53 205 | #define _EOUT 53 206 | #define _RMA 54 207 | #define _MACC 54 208 | #define _RMG 55 209 | #define _RGYRO 55 210 | #define _RMM 56 211 | #define _MMAG 56 212 | #define _ML 57 213 | #define _MALL 57 214 | #define _TS 58 215 | #define _LINES 58 216 | #define _MRS 59 217 | #define _MSENS 59 218 | #define _MZ 60 219 | #define _MSZER 60 220 | #define _PK 61 221 | #define _MPEAK 61 222 | #define _RF 62 223 | #define _MREF 62 224 | #define _FIN 63 225 | #define _GY 64 226 | #define _GYRO 64 227 | #define _ANG 65 228 | #define _ANGLE 65 229 | #define _HCT 66 230 | #define _HCOUNTER 66 231 | #define _HDT 67 232 | #define _HDATA 67 233 | #define _CUI 68 234 | #define _SCC 69 235 | #define _ICL 70 236 | #define _FC 71 237 | #define _SL 72 238 | #define _PHA 73 239 | #define _CRT 74 240 | #define _CHD 75 241 | #define _BMC 76 242 | #define _BMF 77 243 | #define _SOH 79 244 | #define _BMS 78 245 | #define _BSC 80 246 | #define _SWS 81 247 | #define _MGX 82 248 | #define _CW 86 249 | #define _SPL 87 250 | #define _SAC 88 251 | #define _SDC 89 252 | #define _ROM 90 253 | #define _TC 91 254 | #define _POS 92 255 | #define _PSP 93 256 | #define _PAC 94 257 | #define _PDC 95 258 | #define _TSL 96 259 | #define _SW 97 260 | #define _RMP 98 261 | #define _AOM 99 262 | #define _SDM 100 263 | #define _VNM 101 264 | #define _SS 106 265 | #define _SSR 107 266 | #define _CSS 108 267 | #define _CSR 109 268 | #define _DPA 110 269 | #define _SCA 111 270 | #define _STT 112 271 | #define _SNS 113 272 | #define _PWR 114 273 | #define _BEM 115 274 | #define _BIN 116 275 | #define _PXL 117 276 | #define _IMQ 118 277 | #define _SMM 119 278 | #define _MM 120 279 | #define _SNA 121 280 | #define _TRQ 122 281 | #define _FLX 123 282 | #define _FLY 124 283 | #define _MGW 125 284 | #define _MKC 126 285 | #define _MKS 127 286 | #define _MKE 128 287 | #define _MKL 129 288 | #define _MKF 130 289 | #define _MKM 131 290 | #define _MKO 132 291 | #define _MKP 133 292 | #define _MKD 134 293 | #define _MKT 135 294 | #define _MKV 136 295 | */ 296 | //////////////////////////// CONFIGURATION //////////////////////////// 297 | /*#define _EE 0 298 | #define _BKD 1 299 | #define _OVL 2 300 | #define _UVL 3 301 | #define _THLD 4 302 | #define _MXMD 5 303 | #define _PWMF 6 304 | #define _CPRI 7 305 | #define _RWD 8 306 | #define _ECHOF 9 307 | #define _RSBR 10 308 | #define _ACS 11 309 | #define _AMS 12 310 | #define _CLIN 13 311 | #define _DFC 14 312 | #define _DINA 15 313 | #define _DINL 16 314 | #define _DOA 17 315 | #define _DOL 18 316 | #define _AMOD 19 317 | #define _AMIN 20 318 | #define _AMAX 21 319 | #define _ACTR 22 320 | #define _ADB 23 321 | #define _ALIN 24 322 | #define _AINA 25 323 | #define _AMINA 26 324 | #define _AMAXA 27 325 | #define _APOL 28 326 | #define _PMOD 29 327 | #define _PMIN 30 328 | #define _PMAX 31 329 | #define _PCTR 32 330 | #define _PDB 33 331 | #define _PLIN 34 332 | #define _PINA 35 333 | #define _PMINA 36 334 | #define _PMAXA 37 335 | #define _PPOL 38 336 | #define _MMOD 39 337 | #define _MXPF 40 338 | #define _MXPR 41 339 | #define _ALIM 42 340 | #define _ATRIG 43 341 | #define _ATGA 44 342 | #define _ATGD 45 343 | #define _KP 46 344 | #define _KI 47 345 | #define _KD 48 346 | #define _PIDM 49 347 | #define _ICAP 50 348 | #define _MAC 51 349 | #define _MDEC 52 350 | #define _MVEL 53 351 | #define _MXRPM 54 352 | #define _MXTRN 55 353 | #define _CLERD 56 354 | #define _BPOL 57 355 | #define _BLSTD 58 356 | #define _BLFB 59 357 | #define _BHOME 60 358 | #define _BLL 61 359 | #define _BHL 62 360 | #define _BLLA 63 361 | #define _BHLA 64 362 | #define _SPOL 65 363 | #define _OVH 66 364 | #define _ZAIC 67 365 | #define _ZPAO 68 366 | #define _ZPAC 69 367 | #define _ZSMC 70 368 | #define _TELS 71 369 | #define _BRUN 72 370 | #define _EMOD 73 371 | #define _EPPR 74 372 | #define _ELL 75 373 | #define _EHL 76 374 | #define _ELLA 77 375 | #define _EHLA 78 376 | #define _EHOME 79 377 | #define _SKUSE 80 378 | #define _SKMIN 81 379 | #define _SKMAX 82 380 | #define _SKCTR 83 381 | #define _SKDB 84 382 | #define _SKLIN 85 383 | #define _CEN 86 384 | #define _CNOD 87 385 | #define _CBR 88 386 | #define _CHB 89 387 | #define _CAS 90 388 | #define _CLSN 91 389 | #define _CSRT 92 390 | #define _CTPS 93 391 | #define _SCRO 94 392 | #define _BMOD 95 393 | #define _BADJ 96 394 | #define _BADV 97 395 | #define _BZPW 98 396 | #define _BFBK 99 397 | #define _BEE 100 398 | #define _DIM 101 399 | #define _EQS 102 400 | #define _WMOD 103 401 | #define _IPA 104 402 | #define _GWA 105 403 | #define _SBM 106 404 | #define _IPP 107 405 | #define _MACA 108 406 | #define _PDNS 109 407 | #define _SDNS 110 408 | #define _DHCP 111 409 | #define _CTT 112 410 | #define _ZMT 113 411 | #define _ZACC 114 412 | #define _ZGYR 115 413 | #define _ZMAG 116 414 | #define _TPOL 117 415 | #define _TWDT 118 416 | #define _MDIR 119 417 | #define _TXOF 120 418 | #define _TINV 121 419 | #define _TMS 122 420 | #define _TWAD 123 421 | #define _ZADJ 124 422 | #define _PWMM 125 423 | #define _PWMI 126 424 | #define _PWMX 127 425 | #define _AADJ 128 426 | #define _TRF2 129 427 | #define _TRF05 130 428 | #define _TRF5 131 429 | #define _TC2 132 430 | #define _TC05 133 431 | #define _TC5 134 432 | #define _TW2 135 433 | #define _TW05 136 434 | #define _TW5 137 435 | #define _TZER 138 436 | #define _GRNG 139 437 | #define _GYZR 140 438 | #define _KPF 141 439 | #define _KIF 142 440 | #define _TID 143 441 | #define _HDEL 144 442 | #define _CTRIM 145 443 | #define _LEG 146 444 | #define _SSP 147 445 | #define _SST 148 446 | #define _VPH 149 447 | #define _MXS 150 448 | #define _MPW 151 449 | #define _RFC 152 450 | #define _IRR 153 451 | #define _ILLR 154 452 | #define _ILM 155 453 | #define _ILR 156 454 | #define _DMOD 161 455 | #define _MNOD 162 456 | #define _HSM 163 457 | #define _SWD 164 458 | #define _HPO 165 459 | #define _BTNC 166 460 | #define _BCHT 167 461 | #define _BCLT 168 462 | #define _BVHT 169 463 | #define _BVLT 170 464 | #define _BTNT 171 465 | #define _BTHT 172 466 | #define _BTLT 173 467 | #define _BAHT 174 468 | #define _BALT 175 469 | #define _NOMC 176 470 | #define _CNDU 177 471 | #define _CNOV 178 472 | #define _CCHT 179 473 | #define _BPRC 180 474 | #define _BTCD 181 475 | #define _ISRG 182 476 | #define _CRES 183 477 | #define _STYP 184 478 | #define _MXRTR 185 479 | #define _REDL 186 480 | #define _MINOP 187 481 | #define _BECC 189 482 | #define _SVT 190 483 | #define _SSV 191 484 | #define _SSF 192 485 | #define _HEOF 193 486 | #define _HSAT 194 487 | #define _RPWB 195 488 | #define _RPDS 196 489 | #define _RPGR 197 490 | #define _RPWD 198 491 | #define _FCAL 199 492 | #define _ANAM 200 493 | #define _EDEC 201 494 | #define _MDAL 202 495 | #define _MNRPM 203 496 | #define _FSA 204 497 | #define _SCPR 206 498 | #define _SWMAN 205 499 | #define _STO 207 500 | #define _RS485 208 501 | #define _OTL 209 502 | #define _ZSRM 210 503 | #define _ZCAL 211 504 | #define _ZANG 212 505 | #define _MLX 213 506 | #define _SMOD 214 507 | #define _SLL 215 508 | #define _SHL 216 509 | #define _SLLA 217 510 | #define _SHLA 218 511 | #define _SHOME 219 512 | #define _VIND 220 513 | #define _CCFG 221 514 | #define _SMPT 222 515 | #define _TNM 223 516 | #define _SBEE 224 517 | #define _PSA 225 518 | #define _MSP 226 519 | */ 520 | #endif 521 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/include/roboteq_motor_controller_driver/roboteq_motor_controller_driver_node.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOTEQ_MOTOR_CONTROLLER_DRIVER_MAIN_H 2 | #define ROBOTEQ_MOTOR_CONTROLLER_DRIVER_MAIN_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | //! ROS standard msgs 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace roboteq{ 37 | 38 | class Driver 39 | { 40 | public: 41 | //Driver(ros::NodeHandle& nh); 42 | //Driver(); 43 | //~Driver(); 44 | ros::Subscriber cmd_vel_sub; 45 | ros::Publisher read_publisher; 46 | 47 | ros::ServiceServer configsrv; 48 | ros::ServiceServer commandsrv; 49 | ros::ServiceServer maintenancesrv; 50 | //ros::ServiceClient configsrv_client; 51 | //void diff_drive(int speed1, int speed2, float wheel_rad, float wheel_dist, int encoder_coef); 52 | //void cmd_vel_callback(sensor_msgs::Joy::ConstPtr& msg); 53 | void connect(); 54 | void run(); 55 | void roboteq_subscriber(); 56 | void roboteq_publisher(); 57 | void cmd_vel_callback(const geometry_msgs::Twist& msg); 58 | //void initSub(); 59 | 60 | 61 | int channel_number_1; 62 | int channel_number_2; 63 | int frequencyH; 64 | int frequencyL; 65 | int frequencyG; 66 | 67 | 68 | 69 | 70 | void roboteq_services(); 71 | bool configservice(roboteq_motor_controller_driver::config_srv::Request& req, roboteq_motor_controller_driver::config_srv::Response& res); 72 | 73 | bool commandservice(roboteq_motor_controller_driver::command_srv::Request& req, roboteq_motor_controller_driver::command_srv::Response& res); 74 | 75 | bool maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request& req, roboteq_motor_controller_driver::maintenance_srv::Response& res); 76 | 77 | 78 | private: 79 | int baud_rate; 80 | std::string port; 81 | std::string firmware; 82 | int channel; 83 | 84 | 85 | geometry_msgs::TransformStamped tf_msg; 86 | tf::TransformBroadcaster odom_broadcaster; 87 | nav_msgs::Odometry odom_msg; 88 | nav_msgs::Odometry odom; 89 | 90 | enum fault_flag 91 | { 92 | NO_FAULT = 0, 93 | OVERHEAT = 1, 94 | OVERVOLTAGE = 2, 95 | UNDERVOLTAGE = 4, 96 | SHORT_CIRCUIT = 8, 97 | EMERGENCY_STOP = 16, 98 | SETUP_FAULT = 32, 99 | MOSFET_FAILURE = 64, 100 | STARTUP_CONFIG_FAULT = 128, 101 | }; 102 | 103 | 104 | }; 105 | } 106 | #endif // ROBOTEQ_MOTOR_CONTROLLER_DRIVER_MAIN_H 107 | 108 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/launch/diff_odom.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/launch/driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/msg/channel_values.msg: -------------------------------------------------------------------------------- 1 | int64[] value 2 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roboteq_motor_controller_driver 4 | 0.0.0 5 | The roboteq_motor_controller_driver package 6 | 7 | 8 | 9 | 10 | sushrut 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | roslaunch 54 | serial 55 | std_msgs 56 | message_generation 57 | roscpp 58 | serial 59 | std_msgs 60 | roscpp 61 | serial 62 | std_msgs 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/src/command_client.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "command_client"); 7 | ros::NodeHandle nh; 8 | ros::ServiceClient client = nh.serviceClient("command_service"); 9 | roboteq_motor_controller_driver::command_srv srv; 10 | srv.request.userInput = argv[1]; 11 | srv.request.channel = atoll(argv[2]); 12 | srv.request.value = atoll(argv[3]); 13 | if (client.call(srv)) 14 | { 15 | ROS_INFO("success!"); 16 | } 17 | else 18 | { 19 | ROS_ERROR("Failed to call service"); 20 | return 1; 21 | } 22 | 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "config_client"); 7 | ros::NodeHandle nh; 8 | ros::ServiceClient client = nh.serviceClient("config_service"); 9 | roboteq_motor_controller_driver::config_srv srv; 10 | srv.request.userInput = argv[1]; 11 | srv.request.channel = atoll(argv[2]); 12 | srv.request.value = atoll(argv[3]); 13 | if (client.call(srv)) 14 | { 15 | ROS_INFO("success!"); 16 | } 17 | else 18 | { 19 | ROS_ERROR("Failed to call service"); 20 | return 1; 21 | } 22 | 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/src/diff_odom.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "std_msgs/String.h" 3 | #include "std_msgs/Int64.h" 4 | #include "std_msgs/Int16.h" 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | 12 | class Odometry_calc{ 13 | 14 | public: 15 | Odometry_calc(); 16 | 17 | void spin(); 18 | 19 | 20 | private: 21 | ros::NodeHandle n; 22 | ros::Subscriber sub; 23 | ros::Subscriber l_wheel_sub; 24 | ros::Subscriber r_wheel_sub; 25 | ros::Publisher odom_pub; 26 | 27 | tf::TransformBroadcaster odom_broadcaster; 28 | //Encoder related variables 29 | double encoder_min; 30 | double encoder_max; 31 | 32 | double encoder_low_wrap; 33 | double encoder_high_wrap; 34 | 35 | double prev_lencoder; 36 | double prev_rencoder; 37 | 38 | double lmult; 39 | double rmult; 40 | 41 | double left; 42 | double right; 43 | 44 | double rate; 45 | 46 | ros::Duration t_delta; 47 | 48 | ros::Time t_next; 49 | 50 | ros::Time then; 51 | 52 | 53 | double enc_left ; 54 | 55 | double enc_right; 56 | 57 | double ticks_meter; 58 | 59 | double base_width; 60 | 61 | double dx; 62 | 63 | double dr; 64 | 65 | double x_final,y_final, theta_final; 66 | 67 | ros::Time current_time, last_time; 68 | 69 | 70 | void leftencoderCb(const roboteq_motor_controller_driver::channel_values& left_ticks); 71 | 72 | void rightencoderCb(const roboteq_motor_controller_driver::channel_values& right_ticks); 73 | void init_variables(); 74 | 75 | 76 | 77 | 78 | void update(); 79 | }; 80 | 81 | Odometry_calc::Odometry_calc(){ 82 | 83 | 84 | init_variables(); 85 | 86 | ROS_INFO("Started odometry computing node"); 87 | 88 | l_wheel_sub = n.subscribe("/hall_count",1000, &Odometry_calc::leftencoderCb, this); 89 | 90 | r_wheel_sub = n.subscribe("/hall_count",1000, &Odometry_calc::rightencoderCb, this); 91 | 92 | 93 | odom_pub = n.advertise("odom1", 50); 94 | 95 | 96 | 97 | //Retrieving parameters of this node 98 | 99 | } 100 | 101 | void Odometry_calc::init_variables() 102 | { 103 | 104 | 105 | prev_lencoder = 0; 106 | prev_rencoder = 0; 107 | 108 | lmult = 0; 109 | rmult = 0; 110 | 111 | left = 0; 112 | right = 0; 113 | 114 | encoder_min = -65536; 115 | encoder_max = 65536; 116 | 117 | rate = 10; 118 | 119 | ticks_meter = 50; 120 | 121 | base_width = 0.3; 122 | 123 | 124 | 125 | 126 | encoder_low_wrap = ((encoder_max - encoder_min) * 0.3) + encoder_min ; 127 | encoder_high_wrap = ((encoder_max - encoder_min) * 0.7) + encoder_min ; 128 | 129 | t_delta = ros::Duration(1.0 / rate); 130 | t_next = ros::Time::now() + t_delta; 131 | 132 | then = ros::Time::now(); 133 | 134 | 135 | enc_left = 10; 136 | enc_right = 0; 137 | 138 | dx = 0; 139 | dr = 0; 140 | 141 | x_final = 0;y_final=0;theta_final=0; 142 | 143 | current_time = ros::Time::now(); 144 | last_time = ros::Time::now(); 145 | 146 | } 147 | 148 | 149 | 150 | 151 | 152 | //Spin function 153 | void Odometry_calc::spin(){ 154 | 155 | ros::Rate loop_rate(rate); 156 | 157 | while (ros::ok()) 158 | { 159 | update(); 160 | loop_rate.sleep(); 161 | 162 | } 163 | 164 | 165 | } 166 | 167 | //Update function 168 | void Odometry_calc::update(){ 169 | 170 | ros::Time now = ros::Time::now(); 171 | 172 | // ros::Time elapsed; 173 | 174 | double elapsed; 175 | 176 | double d_left, d_right, d, th,x,y; 177 | 178 | 179 | if ( now > t_next) { 180 | 181 | elapsed = now.toSec() - then.toSec(); 182 | 183 | // ROS_INFO_STREAM("elapsed =" << elapsed); 184 | 185 | 186 | 187 | if(enc_left == 0){ 188 | d_left = 0; 189 | d_right = 0; 190 | } 191 | else{ 192 | d_left = (left - enc_left) / ( ticks_meter); 193 | d_right = (right - enc_right) / ( ticks_meter); 194 | } 195 | 196 | enc_left = left; 197 | //ROS_INFO_STREAM(left); 198 | enc_right = right; 199 | 200 | d = (d_left + d_right ) / 2.0; 201 | 202 | ROS_INFO_STREAM(d_left << " : " << d_right); 203 | 204 | 205 | th = ( d_right - d_left ) / base_width; 206 | 207 | dx = d /elapsed; 208 | 209 | dr = th / elapsed; 210 | 211 | 212 | if ( d != 0){ 213 | 214 | x = cos( th ) * d; 215 | //ROS_INFO_STREAM(x); 216 | y = -sin( th ) * d; 217 | // calculate the final position of the robot 218 | x_final = x_final + ( cos( theta_final ) * x - sin( theta_final ) * y ); 219 | y_final = y_final + ( sin( theta_final ) * x + cos( theta_final ) * y ); 220 | 221 | } 222 | 223 | if( th != 0) 224 | theta_final = theta_final + th; 225 | 226 | geometry_msgs::Quaternion odom_quat ; 227 | 228 | odom_quat.x = 0.0; 229 | odom_quat.y = 0.0; 230 | odom_quat.z = 0.0; 231 | 232 | odom_quat.z = sin( theta_final / 2 ); 233 | odom_quat.w = cos( theta_final / 2 ); 234 | 235 | //first, we'll publish the transform over tf 236 | geometry_msgs::TransformStamped odom_trans; 237 | odom_trans.header.stamp = now; 238 | odom_trans.header.frame_id = "odom"; 239 | odom_trans.child_frame_id = "base_footprint"; 240 | 241 | odom_trans.transform.translation.x = x_final; 242 | odom_trans.transform.translation.y = y_final; 243 | odom_trans.transform.translation.z = 0.0; 244 | odom_trans.transform.rotation = odom_quat; 245 | 246 | //send the transform 247 | odom_broadcaster.sendTransform(odom_trans); 248 | 249 | 250 | //next, we'll publish the odometry message over ROS 251 | nav_msgs::Odometry odom; 252 | odom.header.stamp = now; 253 | odom.header.frame_id = "odom"; 254 | 255 | //set the position 256 | odom.pose.pose.position.x = x_final; 257 | odom.pose.pose.position.y = y_final; 258 | odom.pose.pose.position.z = 0.0; 259 | odom.pose.pose.orientation = odom_quat; 260 | 261 | //set the velocity 262 | odom.child_frame_id = "base_footprint"; 263 | odom.twist.twist.linear.x = dx; 264 | odom.twist.twist.linear.y = 0; 265 | odom.twist.twist.angular.z = dr; 266 | 267 | //publish the message 268 | odom_pub.publish(odom); 269 | 270 | then = now; 271 | 272 | // ROS_INFO_STREAM("dx =" << x_final); 273 | 274 | // ROS_INFO_STREAM("dy =" << y_final); 275 | 276 | ros::spinOnce(); 277 | 278 | 279 | } 280 | else { ; } 281 | // ROS_INFO_STREAM("Not in loop"); 282 | 283 | 284 | 285 | 286 | 287 | } 288 | 289 | 290 | 291 | 292 | 293 | void Odometry_calc::leftencoderCb(const roboteq_motor_controller_driver::channel_values& left_ticks) 294 | 295 | { 296 | 297 | // ROS_INFO_STREAM("Left tick" << left_ticks->data); 298 | double enc = left_ticks.value[1]; 299 | 300 | 301 | 302 | if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap)) 303 | { 304 | 305 | lmult = lmult + 1; 306 | } 307 | 308 | 309 | if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap)) 310 | 311 | { 312 | 313 | lmult = lmult - 1; 314 | } 315 | 316 | left = 1.0 * (enc + lmult * (encoder_max - encoder_min )); 317 | 318 | prev_lencoder = enc; 319 | 320 | // ROS_INFO_STREAM("Left " << left); 321 | 322 | } 323 | 324 | 325 | //Right encoder callback 326 | 327 | void Odometry_calc::rightencoderCb(const roboteq_motor_controller_driver::channel_values& right_ticks) 328 | 329 | { 330 | 331 | 332 | // ROS_INFO_STREAM("Right tick" << right_ticks->data); 333 | 334 | 335 | double enc = right_ticks.value[2]; 336 | 337 | if((enc < encoder_low_wrap) && (prev_lencoder > encoder_high_wrap)) 338 | { 339 | 340 | rmult = rmult + 1; 341 | } 342 | 343 | 344 | if((enc > encoder_high_wrap) && (prev_lencoder < encoder_low_wrap)) 345 | 346 | { 347 | 348 | rmult = rmult - 1; 349 | } 350 | 351 | right = 1.0 * (enc + rmult * (encoder_max - encoder_min )); 352 | 353 | prev_rencoder = enc; 354 | 355 | // ROS_INFO_STREAM("Right " << right); 356 | 357 | 358 | 359 | } 360 | 361 | 362 | 363 | 364 | int main(int argc, char **argv) 365 | 366 | { 367 | ros::init(argc, argv,"diff_odom"); 368 | Odometry_calc obj; 369 | obj.spin(); 370 | 371 | 372 | return 0; 373 | 374 | } 375 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | bool Driver::configservice(roboteq_motor_controller_driver::config_srv::Request &request, roboteq_motor_controller_driver::config_srv::Response &response) 4 | { 5 | std::stringstream str; 6 | str << "^" << request.userInput << " " << request.channel << " " << request.value << "_ " << "%\clsav321654987"; 7 | ser.write(str.str()); 8 | response.result = str.str(); 9 | 10 | ROS_INFO_STREAM(response.result); 11 | return true; 12 | } 13 | 14 | bool Driver::commandservice(roboteq_motor_controller_driver::command_srv::Request &request, roboteq_motor_controller_driver::command_srv::Response &response) 15 | { 16 | std::stringstream str; 17 | str << "%" << request.userInput << " "<< "_"; 18 | ser.write(str.str()); 19 | response.result = str.str(); 20 | 21 | ROS_INFO_STREAM(response.result); 22 | return true; 23 | } 24 | 25 | bool Driver::maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request &request, roboteq_motor_controller_driver::maintenance_srv::Response &response) 26 | { 27 | std::stringstream str; 28 | str << "%" << request.userInput << " " << "_"; 29 | ser.write(str.str()); 30 | 31 | response.result = ser.read(ser.available()); 32 | 33 | ROS_INFO_STREAM(response.result); 34 | return true; 35 | } 36 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "maintenance_client"); 7 | ros::NodeHandle nh; 8 | ros::ServiceClient client = nh.serviceClient("maintenance_service"); 9 | roboteq_motor_controller_driver::maintenance_srv srv; 10 | srv.request.userInput = argv[1]; 11 | 12 | 13 | if (client.call(srv)) 14 | { 15 | ROS_INFO("success!"); 16 | } 17 | else 18 | { 19 | ROS_ERROR("Failed to call service"); 20 | return 1; 21 | } 22 | 23 | return 0; 24 | } 25 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/src/roboteq_motor_controller_driver_node.cpp: -------------------------------------------------------------------------------- 1 | // #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | class RoboteqDriver 24 | { 25 | public: 26 | RoboteqDriver() 27 | { 28 | initialize(); //constructor - Initialize 29 | } 30 | 31 | ~RoboteqDriver() 32 | { 33 | if (ser.isOpen()) 34 | { 35 | ser.close(); 36 | } 37 | } 38 | 39 | private: 40 | serial::Serial ser; 41 | std::string port; 42 | int32_t baud; 43 | ros::Publisher read_publisher; 44 | ros::Subscriber cmd_vel_sub; 45 | 46 | int channel_number_1; 47 | int channel_number_2; 48 | int frequencyH; 49 | int frequencyL; 50 | int frequencyG; 51 | ros::NodeHandle nh; 52 | 53 | void initialize() 54 | { 55 | 56 | nh.getParam("port", port); 57 | nh.getParam("baud", baud); 58 | cmd_vel_sub = nh.subscribe("/cmd_vel", 10, &RoboteqDriver::cmd_vel_callback, this); 59 | 60 | connect(); 61 | } 62 | 63 | void connect() 64 | { 65 | 66 | try 67 | { 68 | 69 | ser.setPort(port); 70 | ser.setBaudrate(baud); //get baud as param 71 | serial::Timeout to = serial::Timeout::simpleTimeout(1000); 72 | ser.setTimeout(to); 73 | ser.open(); 74 | } 75 | catch (serial::IOException &e) 76 | { 77 | 78 | ROS_ERROR_STREAM("Unable to open port "); 79 | ROS_INFO_STREAM("Unable to open port"); 80 | ; 81 | } 82 | if (ser.isOpen()) 83 | { 84 | 85 | ROS_INFO_STREAM("Serial Port initialized\""); 86 | } 87 | else 88 | { 89 | // ROS_INFO_STREAM("HI4"); 90 | ROS_INFO_STREAM("Serial Port is not open"); 91 | } 92 | run(); 93 | } 94 | 95 | void cmd_vel_callback(const geometry_msgs::Twist &msg) 96 | { 97 | std::stringstream cmd_sub; 98 | cmd_sub << "!G 1" 99 | << " " << msg.linear.x << "_" 100 | << "!G 2" 101 | << " " << msg.angular.z << "_"; 102 | 103 | ser.write(cmd_sub.str()); 104 | ser.flush(); 105 | ROS_INFO_STREAM(cmd_sub.str()); 106 | } 107 | 108 | ros::NodeHandle n; 109 | ros::ServiceServer configsrv; 110 | ros::ServiceServer commandsrv; 111 | ros::ServiceServer maintenancesrv; 112 | 113 | bool configservice(roboteq_motor_controller_driver::config_srv::Request &request, roboteq_motor_controller_driver::config_srv::Response &response) 114 | { 115 | std::stringstream str; 116 | str << "^" << request.userInput << " " << request.channel << " " << request.value << "_ " 117 | << "%\clsav321654987"; 118 | ser.write(str.str()); 119 | ser.flush(); 120 | response.result = str.str(); 121 | 122 | ROS_INFO_STREAM(response.result); 123 | return true; 124 | } 125 | 126 | bool commandservice(roboteq_motor_controller_driver::command_srv::Request &request, roboteq_motor_controller_driver::command_srv::Response &response) 127 | { 128 | std::stringstream str; 129 | str << "!" << request.userInput << " " << request.channel << " " << request.value << "_"; 130 | ser.write(str.str()); 131 | ser.flush(); 132 | response.result = str.str(); 133 | 134 | ROS_INFO_STREAM(response.result); 135 | return true; 136 | } 137 | 138 | bool maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request &request, roboteq_motor_controller_driver::maintenance_srv::Response &response) 139 | { 140 | std::stringstream str; 141 | str << "%" << request.userInput << " " 142 | << "_"; 143 | ser.write(str.str()); 144 | ser.flush(); 145 | response.result = ser.read(ser.available()); 146 | 147 | ROS_INFO_STREAM(response.result); 148 | return true; 149 | } 150 | 151 | void initialize_services() 152 | { 153 | n = ros::NodeHandle(); 154 | configsrv = n.advertiseService("config_service", &RoboteqDriver::configservice, this); 155 | commandsrv = n.advertiseService("command_service", &RoboteqDriver::commandservice, this); 156 | maintenancesrv = n.advertiseService("maintenance_service", &RoboteqDriver::maintenanceservice, this); 157 | } 158 | 159 | void run() 160 | { 161 | initialize_services(); 162 | std_msgs::String str1; 163 | ros::NodeHandle nh; 164 | nh.getParam("frequencyH", frequencyH); 165 | nh.getParam("frequencyL", frequencyL); 166 | nh.getParam("frequencyG", frequencyG); 167 | 168 | typedef std::string Key; 169 | typedef std::string Val; 170 | std::map map_sH; 171 | nh.getParam("queryH", map_sH); 172 | 173 | std::stringstream ss0; 174 | std::stringstream ss1; 175 | std::stringstream ss2; 176 | std::stringstream ss3; 177 | std::vector KH_vector; 178 | 179 | ss0 << "^echof 1_"; 180 | ss1 << "# c_/\"DH?\",\"?\""; 181 | for (std::map::iterator iter = map_sH.begin(); iter != map_sH.end(); ++iter) 182 | { 183 | Key KH = iter->first; 184 | 185 | KH_vector.push_back(KH); 186 | 187 | Val VH = iter->second; 188 | 189 | ss1 << VH << "_"; 190 | } 191 | ss1 << "# " << frequencyH << "_"; 192 | 193 | std::vector publisherVecH; 194 | for (int i = 0; i < KH_vector.size(); i++) 195 | { 196 | publisherVecH.push_back(nh.advertise(KH_vector[i], 100)); 197 | } 198 | 199 | ser.write(ss0.str()); 200 | ser.write(ss1.str()); 201 | ser.write(ss2.str()); 202 | ser.write(ss3.str()); 203 | 204 | ser.flush(); 205 | int count = 0; 206 | read_publisher = nh.advertise("read", 1000); 207 | sleep(2); 208 | ros::Rate loop_rate(5); 209 | while (ros::ok()) 210 | { 211 | 212 | ros::spinOnce(); 213 | if (ser.available()) 214 | { 215 | 216 | std_msgs::String result; 217 | result.data = ser.read(ser.available()); 218 | 219 | read_publisher.publish(result); 220 | boost::replace_all(result.data, "\r", ""); 221 | boost::replace_all(result.data, "+", ""); 222 | 223 | std::vector fields; 224 | 225 | std::vector Field9; 226 | boost::split(fields, result.data, boost::algorithm::is_any_of("D")); 227 | 228 | std::vector fields_H; 229 | boost::split(fields_H, fields[1], boost::algorithm::is_any_of("?")); 230 | 231 | if (fields_H[0] == "H") 232 | { 233 | 234 | for (int i = 0; i < publisherVecH.size(); ++i) 235 | { 236 | 237 | std::vector sub_fields_H; 238 | 239 | boost::split(sub_fields_H, fields_H[i + 1], boost::algorithm::is_any_of(":")); 240 | roboteq_motor_controller_driver::channel_values Q1; 241 | 242 | for (int j = 0; j < sub_fields_H.size(); j++) 243 | { 244 | 245 | try 246 | { 247 | Q1.value.push_back(boost::lexical_cast(sub_fields_H[j])); 248 | } 249 | catch (const std::exception &e) 250 | { 251 | count++; 252 | if (count > 10) 253 | { 254 | ROS_INFO_STREAM("Garbage data on Serial"); 255 | //std::cerr << e.what() << '\n'; 256 | } 257 | } 258 | } 259 | 260 | publisherVecH[i].publish(Q1); 261 | } 262 | } 263 | } 264 | loop_rate.sleep(); 265 | } 266 | } 267 | }; 268 | 269 | int main(int argc, char **argv) 270 | { 271 | ros::init(argc, argv, "roboteq_motor_controller_driver"); 272 | 273 | RoboteqDriver driver; 274 | 275 | ros::waitForShutdown(); 276 | 277 | return 0; 278 | } 279 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/srv/command_srv.srv: -------------------------------------------------------------------------------- 1 | string userInput 2 | int64 channel 3 | int64 value 4 | --- 5 | string result 6 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/srv/config_srv.srv: -------------------------------------------------------------------------------- 1 | string userInput 2 | int64 channel 3 | int64 value 4 | --- 5 | string result 6 | -------------------------------------------------------------------------------- /ROS-Driver-Update/roboteq_motor_controller_driver/srv/maintenance_srv.srv: -------------------------------------------------------------------------------- 1 | string userInput 2 | --- 3 | string result 4 | --------------------------------------------------------------------------------