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 56% rename from ROS-Driver-Update/README.md rename to README.md index 980ef41..791e176 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 -``` +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)](?). -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 - - -``` -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 +catkin build roboteq_controller +source devel/setup.bash +roslaunch roboteq_controller driver.launch +``` 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/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/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/ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt b/roboteq_controller/CMakeLists.txt similarity index 90% rename from ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt rename to roboteq_controller/CMakeLists.txt index 7853e9c..15e9be7 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/CMakeLists.txt +++ b/roboteq_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 2.8.3) -project(roboteq_motor_controller_driver) +project(roboteq_controller) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) @@ -121,7 +121,7 @@ catkin_package( ## Your package locations should be listed before other locations include_directories( include - ${roboteq_motor_controller_driver_INCLUDE_DIRS} + ${roboteq_controller_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ) @@ -139,9 +139,9 @@ include_directories( ## 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 +add_executable(roboteq_controller_node #src/main.cpp - src/roboteq_motor_controller_driver_node.cpp) + src/roboteq_controller_node.cpp) #src/configservice.cpp) ## Rename C++ executable without prefix @@ -152,26 +152,22 @@ add_executable(roboteq_motor_controller_driver_node ## 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) +add_dependencies(roboteq_controller_node roboteq_controller_generate_messages_cpp) -target_link_libraries(roboteq_motor_controller_driver_node +target_link_libraries(roboteq_controller_node ${catkin_LIBRARIES} ) add_executable(config_client src/config_client.cpp) - - add_dependencies(config_client roboteq_motor_controller_driver_generate_messages_cpp) - +add_dependencies(config_client roboteq_controller_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) - +add_dependencies(command_client roboteq_controller_generate_messages_cpp) target_link_libraries(command_client ${catkin_LIBRARIES} ) @@ -179,19 +175,15 @@ target_link_libraries(command_client add_executable(maintenance_client src/maintenance_client.cpp) - - add_dependencies(maintenance_client roboteq_motor_controller_driver_generate_messages_cpp) - +add_dependencies(maintenance_client roboteq_controller_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 +add_executable(diff_odom_node + src/diff_odom_node.cpp) +add_dependencies(diff_odom_node roboteq_controller_generate_messages_cpp) +target_link_libraries(diff_odom_node ${catkin_LIBRARIES} ) @@ -228,7 +220,7 @@ install(DIRECTORY include/${PROJECT_NAME}/ ) ## 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 +install(TARGETS roboteq_controller_node ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} diff --git a/roboteq_controller/config/query.yaml b/roboteq_controller/config/query.yaml new file mode 100644 index 0000000..b154100 --- /dev/null +++ b/roboteq_controller/config/query.yaml @@ -0,0 +1,38 @@ +serial_port : "/dev/sensors/XDC2460" # Udev rule to make a virtual port that's associated to specific hw +baudrate : 115200 +cmd_vel_topic: "/cmd_vel_roboteq" + +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 # [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..2d99142 --- /dev/null +++ b/roboteq_controller/include/roboteq_controller/roboteq_controller_node.h @@ -0,0 +1,91 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +#include "roboteq_controller/querylist.h" +#include "roboteq_controller/channel_values.h" +#include "roboteq_controller/config_srv.h" +#include "roboteq_controller/command_srv.h" +#include "roboteq_controller/maintenance_srv.h" + + +class RoboteqDriver +{ +public: + RoboteqDriver(ros::NodeHandle, ros::NodeHandle); + ~RoboteqDriver(){ + if (ser_.isOpen()){ + ser_.close(); + } + } + +private: + ros::NodeHandle nh_, nh_priv_; + + ros::ServiceServer configsrv_; + ros::ServiceServer commandsrv_; + ros::ServiceServer maintenancesrv_; + + // Serial + serial::Serial ser_; + std::string serial_port_; + int32_t baudrate_; + + // Pub & Sub + ros::Subscriber cmd_vel_sub_; + ros::Publisher serial_read_pub_; + std::vector query_pub_; + + ros::Timer timer_pub_; + + bool closed_loop_, + diff_drive_mode_; + + double wheel_circumference_, + track_width_, + max_rpm_; + + std::string cmd_vel_topic_; + + // int channel_number_1; + // int channel_number_2; + int frequency_; + std::mutex locker; + + void cmdSetup(); + void cmdVelCallback(const geometry_msgs::Twist &); + void powerCmdCallback(const geometry_msgs::Twist &); + bool configService(roboteq_controller::config_srv::Request &, roboteq_controller::config_srv::Response &); + bool commandService(roboteq_controller::command_srv::Request &, roboteq_controller::command_srv::Response &); + bool maintenanceService(roboteq_controller::maintenance_srv::Request &, roboteq_controller::maintenance_srv::Response &); + void initializeServices(); + void run(); + + void formQuery(std::string, + std::map &, + std::vector &, + std::stringstream &); + + void queryCallback (const ros::TimerEvent &); + +}; \ No newline at end of file diff --git a/roboteq_controller/launch/diff_odom.launch b/roboteq_controller/launch/diff_odom.launch new file mode 100644 index 0000000..bb21401 --- /dev/null +++ b/roboteq_controller/launch/diff_odom.launch @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/roboteq_controller/launch/driver.launch b/roboteq_controller/launch/driver.launch new file mode 100644 index 0000000..08eecd8 --- /dev/null +++ b/roboteq_controller/launch/driver.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/roboteq_controller/msg/channel_values.msg b/roboteq_controller/msg/channel_values.msg new file mode 100644 index 0000000..8280bc0 --- /dev/null +++ b/roboteq_controller/msg/channel_values.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +int64[] value \ No newline at end of file diff --git a/roboteq_controller/package.xml b/roboteq_controller/package.xml new file mode 100644 index 0000000..8f51428 --- /dev/null +++ b/roboteq_controller/package.xml @@ -0,0 +1,30 @@ + + + roboteq_controller + 0.0.5 + The roboteq_controller package + sushrut + Doan Nguyen + + MIT + + 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/roboteq_controller/src/command_client.cpp similarity index 63% rename from ROS-Driver-Update/roboteq_motor_controller_driver/src/command_client.cpp rename to roboteq_controller/src/command_client.cpp index 2b1858d..d590014 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/command_client.cpp +++ b/roboteq_controller/src/command_client.cpp @@ -1,12 +1,12 @@ #include "ros/ros.h" -#include +#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; + ros::ServiceClient client = nh.serviceClient("command_service"); + roboteq_controller::command_srv srv; srv.request.userInput = argv[1]; srv.request.channel = atoll(argv[2]); srv.request.value = atoll(argv[3]); diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp b/roboteq_controller/src/config_client.cpp similarity index 64% rename from ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp rename to roboteq_controller/src/config_client.cpp index 664d2b1..8613bf5 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/config_client.cpp +++ b/roboteq_controller/src/config_client.cpp @@ -1,12 +1,12 @@ #include "ros/ros.h" -#include +#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; + ros::ServiceClient client = nh.serviceClient("config_service"); + roboteq_controller::config_srv srv; srv.request.userInput = argv[1]; srv.request.channel = atoll(argv[2]); srv.request.value = atoll(argv[3]); diff --git a/roboteq_controller/src/diff_odom_node.cpp b/roboteq_controller/src/diff_odom_node.cpp new file mode 100644 index 0000000..68ef51e --- /dev/null +++ b/roboteq_controller/src/diff_odom_node.cpp @@ -0,0 +1,138 @@ +#include "roboteq_controller/diff_odom_node.h" + +static const std::string tag {"[RoboteQ-Odom] "}; + +RoboteqOdom::RoboteqOdom(ros::NodeHandle nh, ros::NodeHandle nh_priv): + nh_(nh), + nh_priv_(nh_priv), + encoder_right_prev_(0), + encoder_left_prev_(0), + encoder_left_(0), + encoder_right_(0), + encoder_min_(std::numeric_limits::min()), + encoder_max_(std::numeric_limits::max()){ + + nh_priv_.param("odom_frame", odom_frame_, "odom"); + nh_priv_.param("child_frame", child_frame_, "base_footprint"); + + nh_priv_.getParam("wheel_circumference", wheel_circumference_); + if (wheel_circumference_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! wheel_circumference need to be greater than zero."); + } + nh_priv.getParam("track_width", track_width_); + if (track_width_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! track_width need to be greater than zero."); + } + nh_priv.getParam("encoder_resolution", encoder_resolution_); + if ( encoder_resolution_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! encoder_resolution need to be greater than zero."); + } + nh_priv.getParam("max_rpm", max_rpm_); + if ( max_rpm_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! max_rpm need to be greater than zero."); + } + + max_angular_vel_ = max_rpm_/60. *2 * M_PI; + + encoder_sub_ = nh_.subscribe("encoder_count", 10, &RoboteqOdom::encoderCallback, this); + odom_pub_ = nh_.advertise("odom", 50); + + odom_.child_frame_id = child_frame_; + odom_.header.frame_id= odom_frame_; + + current_time_ = ros::Time::now(); + last_time_ = ros::Time::now(); +} + + +void RoboteqOdom::encoderCallback(const roboteq_controller::channel_values& tick){ + std::lock_guard lock(locker); + + int array_size = sizeof(tick.value)/sizeof(tick.value[0]); + assert(array_size == 2); + + encoder_left_prev_ = encoder_left_; + encoder_right_prev_= encoder_right_; + + encoder_left_ = tick.value[0]; + encoder_right_ = tick.value[1]; + + ros::Duration duration = tick.header.stamp - odom_.header.stamp; + double dt = duration.sec + (double)duration.nsec/1.0e-9; + + // Linear velocity of two wheels + double vel_left = 2* M_PI * (encoder_left_ - encoder_left_prev_)/encoder_resolution_/dt; + double vel_right= 2* M_PI * (encoder_right_- encoder_right_prev_)/encoder_resolution_/dt; + + assert (vel_left <= max_angular_vel_ && vel_right <= max_angular_vel_); + + // Linear velocity and angular velocity of robot + double velocity = wheel_circumference_/4 * (vel_left + vel_right); + double angular = wheel_circumference_/track_width_/2 *(vel_right - vel_left); + + tf::Quaternion q( + odom_.pose.pose.orientation.x, + odom_.pose.pose.orientation.y, + odom_.pose.pose.orientation.z, + odom_.pose.pose.orientation.w); + tf::Matrix3x3 m(q); + double roll, pitch, yaw; + m.getRPY(roll, pitch, yaw); + + // Velocity in x and y directions + double velocity_x = velocity * cos(yaw); + double velocity_y = velocity * sin(yaw); + + // Update robot's pose + tf2::Quaternion q_rot; + q_rot.setRPY(roll, pitch, yaw + angular * dt); + + + // geometry_msgs::Quaternion odom_quat ; + // //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); + + //Publish the odometry message over ROS + + odom_.header.stamp = tick.header.stamp; + //set the position + odom_.pose.pose.position.x += velocity_x * dt; + odom_.pose.pose.position.y += velocity_y * dt; + odom_.pose.pose.orientation.x = q_rot.getX(); + odom_.pose.pose.orientation.y = q_rot.getY(); + odom_.pose.pose.orientation.z = q_rot.getZ(); + odom_.pose.pose.orientation.w = q_rot.getW(); + + //set the velocity + odom_.twist.twist.linear.x = velocity_x; + odom_.twist.twist.linear.y = velocity_y; + odom_.twist.twist.angular.z = angular; + + //publish the message + odom_pub_.publish(odom_); +} + + +int main(int argc, char **argv) + +{ + ros::init(argc, argv,"diff_odom_node"); + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + RoboteqOdom obj(nh, nh_priv); + + ros::spin(); + return 0; + +} diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp b/roboteq_controller/src/init_service.cpp similarity index 52% rename from ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp rename to roboteq_controller/src/init_service.cpp index 02fe9cb..6925e11 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/init_service.cpp +++ b/roboteq_controller/src/init_service.cpp @@ -1,6 +1,6 @@ -#include +#include "roboteq_controller/init_service.h" -bool Driver::configservice(roboteq_motor_controller_driver::config_srv::Request &request, roboteq_motor_controller_driver::config_srv::Response &response) +bool Driver::configservice(roboteq_controller::config_srv::Request &request, roboteq_controller::config_srv::Response &response) { std::stringstream str; str << "^" << request.userInput << " " << request.channel << " " << request.value << "_ " << "%\clsav321654987"; @@ -11,7 +11,7 @@ bool Driver::configservice(roboteq_motor_controller_driver::config_srv::Request return true; } -bool Driver::commandservice(roboteq_motor_controller_driver::command_srv::Request &request, roboteq_motor_controller_driver::command_srv::Response &response) +bool Driver::commandservice(roboteq_controller::command_srv::Request &request, roboteq_controller::command_srv::Response &response) { std::stringstream str; str << "%" << request.userInput << " "<< "_"; @@ -22,7 +22,7 @@ bool Driver::commandservice(roboteq_motor_controller_driver::command_srv::Reques return true; } -bool Driver::maintenanceservice(roboteq_motor_controller_driver::maintenance_srv::Request &request, roboteq_motor_controller_driver::maintenance_srv::Response &response) +bool Driver::maintenanceservice(roboteq_controller::maintenance_srv::Request &request, roboteq_controller::maintenance_srv::Response &response) { std::stringstream str; str << "%" << request.userInput << " " << "_"; diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp b/roboteq_controller/src/maintenance_client.cpp similarity index 57% rename from ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp rename to roboteq_controller/src/maintenance_client.cpp index 8967190..bb4cb3e 100644 --- a/ROS-Driver-Update/roboteq_motor_controller_driver/src/maintenance_client.cpp +++ b/roboteq_controller/src/maintenance_client.cpp @@ -1,12 +1,12 @@ #include "ros/ros.h" -#include +#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; + ros::ServiceClient client = nh.serviceClient("maintenance_service"); + roboteq_controller::maintenance_srv srv; srv.request.userInput = argv[1]; diff --git a/roboteq_controller/src/roboteq_controller_node.cpp b/roboteq_controller/src/roboteq_controller_node.cpp new file mode 100644 index 0000000..ba60510 --- /dev/null +++ b/roboteq_controller/src/roboteq_controller_node.cpp @@ -0,0 +1,346 @@ +#include "roboteq_controller/roboteq_controller_node.h" + +static const std::string tag {"[RoboteQ] "}; + + +RoboteqDriver::RoboteqDriver(ros::NodeHandle nh, ros::NodeHandle nh_priv): + nh_(nh), + nh_priv_(nh_priv), + wheel_circumference_(0.), + track_width_(0.), + max_rpm_(0.), + frequency_(0){ + + nh_priv.param("serial_port", serial_port_, "dev/ttyUSB0"); + nh_priv.param("baudrate", baudrate_, 112500); + + nh_priv_.param("closed_loop", closed_loop_, false); + nh_priv_.param("diff_drive_mode", diff_drive_mode_, false); + if (close){ + ROS_WARN_STREAM(tag << "In CLOSED-LOOP mode!!!!"); + } + else{ + ROS_WARN_STREAM(tag << "In OPEN-LOOP mode!!!!"); + } + + nh_priv_.getParam("wheel_circumference", wheel_circumference_); + if (wheel_circumference_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! wheel_circumference need to be greater than zero."); + } + nh_priv.getParam("track_width", track_width_); + if (track_width_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! track_width need to be greater than zero."); + } + nh_priv.getParam("max_rpm", max_rpm_); + if ( max_rpm_ <=0.0 ){ + ROS_ERROR_STREAM(tag << "Inproper configuration! max_rpm need to be greater than zero."); + } + + nh_priv_.param("cmd_vel_topic", cmd_vel_topic_, "/cmd_vel"); + if (diff_drive_mode_){ + cmd_vel_sub_ = nh_.subscribe(cmd_vel_topic_, 10, &RoboteqDriver::cmdVelCallback, this); + } + else{ + cmd_vel_sub_ = nh_.subscribe(cmd_vel_topic_, 10, &RoboteqDriver::powerCmdCallback, this); + } + + // 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){ + ROS_ERROR_STREAM(tag << "Unable to open port " << serial_port_); + ROS_INFO_STREAM(tag << "Unable to open port" << serial_port_); + ros::shutdown(); + } + + if (ser_.isOpen()){ + ROS_INFO_STREAM(tag << "Serial Port " << serial_port_ << " initialized"); + } + else{ + ROS_INFO_STREAM(tag << "Serial Port " << serial_port_ << " is not open"); + ros::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(); + nh_priv_.getParam("frequency", frequency_); + + if (frequency_ > 0){ + std::stringstream ss0, ss1; + ss0 << "^echof 1_"; + ss1 << "# c_/\"DH?\",\"?\""; + std::map query_map; + formQuery("query", query_map, query_pub_, ss1); + + ss1 << "# " << frequency_ << "_"; + + ser_.write(ss0.str()); + ser_.write(ss1.str()); + ser_.flush(); + } + + serial_read_pub_ = nh_.advertise("read", 1000); + + if (frequency_ > 0){ + timer_pub_ = nh_.createTimer(ros::Duration(frequency_/ 1000.), &RoboteqDriver::queryCallback, this); + } +} + + +void RoboteqDriver::powerCmdCallback(const geometry_msgs::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(); + ROS_INFO("[ROBOTEQ] left: %9.3f right: %9.3f", msg.linear.x, msg.angular.z); + // ROS_INFO_STREAM(cmd_str.str()); +} + + +void RoboteqDriver::cmdVelCallback(const geometry_msgs::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; + + // ROS_INFO("[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_); + + ROS_INFO("[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_; + + ROS_INFO("[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(); + // ROS_INFO_STREAM(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(); + + ROS_INFO_STREAM(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(); + + ROS_INFO_STREAM(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()); + + ROS_INFO_STREAM(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::formQuery(std::string param, + std::map &queries, + std::vector &pubs, + std::stringstream &ser_str){ + nh_priv_.getParam(param, queries); + for (std::map::iterator iter = queries.begin(); iter != queries.end(); iter++){ + ROS_INFO_STREAM(tag << "Publish topic: " << iter->first); + pubs.push_back(nh_.advertise(iter->first, 100)); + + std::string cmd = iter->second; + ser_str << cmd << "_"; + } +} + + +void RoboteqDriver::queryCallback(const ros::TimerEvent &){ + int count = 0; + ros::Time current_time = ros::Time::now(); + if (ser_.available()){ + + + std_msgs::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){ + + ROS_ERROR_STREAM(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'; + ROS_ERROR_STREAM(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_controller::channel_values 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){ + ROS_ERROR_STREAM(tag << "Garbage data on Serial"); + std::cerr << e.what() << '\n'; + } + } + query_pub_[i].publish(msg); + } + } + } + else{ + ROS_WARN_STREAM(tag << "Unknown:{" << result.data << "}"); + } + } +} + + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "roboteq_controller"); + ros::NodeHandle nh; + ros::NodeHandle nh_priv("~"); + RoboteqDriver driver(nh, nh_priv); + ros::MultiThreadedSpinner spinner(4); + spinner.spin(); + ros::waitForShutdown(); + + return 0; +} diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/command_srv.srv b/roboteq_controller/srv/command_srv.srv similarity index 100% rename from ROS-Driver-Update/roboteq_motor_controller_driver/srv/command_srv.srv rename to roboteq_controller/srv/command_srv.srv diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/config_srv.srv b/roboteq_controller/srv/config_srv.srv similarity index 100% rename from ROS-Driver-Update/roboteq_motor_controller_driver/srv/config_srv.srv rename to roboteq_controller/srv/config_srv.srv diff --git a/ROS-Driver-Update/roboteq_motor_controller_driver/srv/maintenance_srv.srv b/roboteq_controller/srv/maintenance_srv.srv similarity index 100% rename from ROS-Driver-Update/roboteq_motor_controller_driver/srv/maintenance_srv.srv rename to roboteq_controller/srv/maintenance_srv.srv