From 92aa2ff8b58c8233b6cb428e69e18536572bc3be Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 26 Apr 2019 16:56:59 +0900 Subject: [PATCH 01/41] Compiler passed with new structure ros_whill skelton --- CMakeLists.txt | 27 +-- src/odom.h | 60 ------- src/ros_whill.cpp | 125 ++++++++++++++ src/whill/CMakeLists.txt | 8 + src/whill/Packet.cpp | 99 +++++++++++ src/whill/PacketParser.cpp | 90 ++++++++++ src/whill/PacketReceiver.cpp | 92 ++++++++++ src/whill/WHILL.cpp | 106 ++++++++++++ src/whill/WHILL.h | 199 ++++++++++++++++++++++ src/whill/whill_commands.cpp | 101 +++++++++++ src/whill_modelc/com_whill.cpp | 291 -------------------------------- src/whill_modelc/com_whill.h | 58 ------- src/whill_modelc/uart.cpp | 172 ------------------- src/whill_modelc/uart.h | 37 ---- src/whill_modelc_controller.cpp | 189 --------------------- 15 files changed, 834 insertions(+), 820 deletions(-) delete mode 100644 src/odom.h create mode 100644 src/ros_whill.cpp create mode 100644 src/whill/CMakeLists.txt create mode 100644 src/whill/Packet.cpp create mode 100644 src/whill/PacketParser.cpp create mode 100644 src/whill/PacketReceiver.cpp create mode 100644 src/whill/WHILL.cpp create mode 100644 src/whill/WHILL.h create mode 100644 src/whill/whill_commands.cpp delete mode 100644 src/whill_modelc/com_whill.cpp delete mode 100644 src/whill_modelc/com_whill.h delete mode 100644 src/whill_modelc/uart.cpp delete mode 100644 src/whill_modelc/uart.h delete mode 100644 src/whill_modelc_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ec3c3e3..b5d341c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -13,12 +13,15 @@ find_package(catkin REQUIRED COMPONENTS rospy std_msgs tf + serial ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) +add_subdirectory(./src/whill) + ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html @@ -51,16 +54,12 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - msgWhillModelC.msg - msgWhillSpeedProfile.msg ) ## Generate services in the 'srv' folder add_service_files( FILES srvSetSpeedProfile.srv - srvSetPower.srv - srvSetBatteryVoltageOut.srv ) ## Generate actions in the 'action' folder @@ -137,8 +136,8 @@ 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/ros_whill_node.cpp) -add_executable(whill_modelc_publisher src/whill_modelc_publisher.cpp src/whill_modelc/uart.cpp src/whill_modelc/com_whill.cpp src/odom.cpp) -add_executable(whill_modelc_controller src/whill_modelc_controller.cpp src/whill_modelc/uart.cpp src/whill_modelc/com_whill.cpp) +add_executable(ros_whill src/ros_whill.cpp) + ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the @@ -149,15 +148,17 @@ add_executable(whill_modelc_controller src/whill_modelc_controller.cpp src/whill ## Add cmake target dependencies of the executable ## same as for the library above # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(whill_modelc_publisher ${PROJECT_NAME}_generate_messages_cpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_dependencies(whill_modelc_controller ${PROJECT_NAME}_generate_messages_cpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(whill_modelc_publisher ${PROJECT_NAME}_generate_messages_cpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +# add_dependencies(whill_modelc_controller ${PROJECT_NAME}_generate_messages_cpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(ros_whill ${PROJECT_NAME}_generate_messages_cpp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) -target_link_libraries(whill_modelc_publisher ${catkin_LIBRARIES}) -target_link_libraries(whill_modelc_controller ${catkin_LIBRARIES}) +target_link_libraries(ros_whill + lib_whill + ${catkin_LIBRARIES} + ) + +#target_link_libraries(ros_whill ${catkin_LIBRARIES}) ############# ## Install ## diff --git a/src/odom.h b/src/odom.h deleted file mode 100644 index 835cb49..0000000 --- a/src/odom.h +++ /dev/null @@ -1,60 +0,0 @@ -/* -MIT License - -Copyright (c) 2018 WHILL inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#ifndef __ODOM_H__ -#define __ODOM_H__ - - - #include "sensor_msgs/JointState.h" - #include "nav_msgs/Odometry.h" - - class Odometry{ - private: - long double confineRadian(long double rad); - - typedef struct{ - long double x; - long double y; - long double theta; - }Space2D; - - static constexpr double wheel_radius_ = 0.1325; - static constexpr double wheel_tread_ = 0.248; - - Space2D pose; - Space2D velocity; - - public: - Odometry(); - void update(sensor_msgs::JointState joint,double dt); - void set(Space2D pose); - void reset(); - - nav_msgs::Odometry getROSOdometry(); - geometry_msgs::TransformStamped getROSTransformStamped(); - Space2D getOdom(); - }; - - -#endif \ No newline at end of file diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp new file mode 100644 index 0000000..a409afa --- /dev/null +++ b/src/ros_whill.cpp @@ -0,0 +1,125 @@ +/* +MIT License + +Copyright (c) 2018 WHILL inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +#include +#include + +#include "ros/ros.h" +#include "sensor_msgs/Joy.h" +#include "sensor_msgs/JointState.h" +#include "sensor_msgs/Imu.h" +#include "sensor_msgs/BatteryState.h" +#include "nav_msgs/Odometry.h" + +#include "whill/WHILL.h" + +// #include "./includes/subscriber.hpp" +// #include "./includes/services.hpp" + +#include "serial/serial.h" + +int main(int argc, char **argv) +{ + // ROS setup + ros::init(argc, argv, "whill"); + ros::NodeHandle nh("~"); + + std::string serialport; + nh.param("serialport", serialport, "/dev/ttyUSB0"); + + bool activate_experimental; + nh.param("activate_experimental", activate_experimental, false); + + // Node Param + int send_interval = 10; + nh.getParam("send_interval", send_interval); + if (send_interval < 10) + { + ROS_WARN("Too short interval. Set interval > 10"); + nh.setParam("/whill_modelc_publisher/send_interval", 10); + send_interval = 10; + } + ROS_INFO("param: send_interval=%d", send_interval); + + // // Services + // ros::ServiceServer set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); + // //ros::ServiceServer service = nh.advertiseService("odom/clear", &clearOdom); + + // // Subscribers + // ros::Subscriber control_joystick_subscriber = nh.subscribe("controller/joy", 100, control_joystick_callback); // Defined in subscriber.cpp + + // // Publishers + // ros::Publisher joystick_state_publisher = nh.advertise("states/joy", 100); + // ros::Publisher jointstate_publisher = nh.advertise("states/jointState", 100); + // ros::Publisher imu_publisher = nh.advertise("states/imu", 100); + // ros::Publisher battery_state_publisher = nh.advertise("states/batteryState", 100); + // ros::Publisher odom_publisher = nh.advertise("odom", 100); + + if (activate_experimental) + { + //Services + + //Subscribers + //ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, control_cmd_vel_callback); + + // Publishers + } + + usleep(100); + + std::string port = "/dev/ttyUSB0"; + unsigned long baud = 38400; + serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(100)); + + ros::AsyncSpinner spinner(1); + spinner.start(); + ros::Rate rate(1); + + // + // // cmd[0] = START; + // cmd[1] = data_set_num; + // cmd[2] = (char)(t >> 8); + // cmd[3] = (char)t; + // cmd[4] = speed_mode; // added on Model C + /// + //sendStartSendingData(my_serial, send_interval, DATASET_NUM_ONE, SPEED_MODE); + usleep(2000); + + while (ros::ok()) + { + // std::vector received; + // //size_t length = my_serial.read(received,20); + // std::cout << length << ", String read: " << std::endl; + // for (std::vector::iterator i = begin(received); i != end(received); ++i) + // { + // printf("%02x,", (unsigned int)*i); + // } + //ROS_INFO("sleep()"); + rate.sleep(); + } + + spinner.stop(); + + return 0; +} \ No newline at end of file diff --git a/src/whill/CMakeLists.txt b/src/whill/CMakeLists.txt new file mode 100644 index 0000000..f6f24eb --- /dev/null +++ b/src/whill/CMakeLists.txt @@ -0,0 +1,8 @@ +cmake_minimum_required(VERSION 2.8) +add_library(lib_whill STATIC + Packet.cpp + PacketParser.cpp + PacketReceiver.cpp + whill_commands.cpp + WHILL.cpp +) \ No newline at end of file diff --git a/src/whill/Packet.cpp b/src/whill/Packet.cpp new file mode 100644 index 0000000..99c101b --- /dev/null +++ b/src/whill/Packet.cpp @@ -0,0 +1,99 @@ +/* +The MIT License (MIT) + +Copyright (c) 2018 WHILL, + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include "WHILL.h" +#include + +WHILL::Packet::Packet(){ + protocol_sign = 0xAF; + len = 1; + build(); +} + +WHILL::Packet::Packet(unsigned char payload[],int size){ + protocol_sign = 0xAF; + len = size + 1; + memcpy(this->payload,payload,size); + build(); +} + +bool WHILL::Packet::is_valid(){ + return getCalculatedCS() == cs; +} + +int WHILL::Packet::rawLength(){ + return 2 + len; // protocol_sign + len + (the length of payload and cs) +} + +bool WHILL::Packet::setRaw(unsigned char* raw,int whole_length){ + + protocol_sign = raw[0]; + len = raw[1]; + + int prefix = 2; + int i = 0; + for(i=0;ics = getCalculatedCS(); +} + +int WHILL::Packet::getRaw(unsigned char* raw){ + + int whole_length = 0; + + raw[0] = protocol_sign; + raw[1] = len; + + int prefix = 2; + int i = 0; + for(i=0;iwhill = whill; +} + +int WHILL::PacketParser::parsePacket(Packet* packet){ + if(!(packet->is_valid()))return -1; + + switch(packet->payload[0]){ // Read Command ID + + case 0x00: // Data set 0 + parseDataset0(packet); + break; + + case 0x01: // Data set 1 + parseDataset1(packet); + break; + + case 0x52: // Response of power WHILL on. + if(whill!=NULL){ + whill->fire_callback(CALLBACK_POWER_ON); + } + break; + + default: + return -1; // Unknown Command + } + + return 0; + +} + + +void WHILL::PacketParser::parseDataset0(Packet* packet){ + if(whill == NULL) return; + whill->fire_callback(CALLBACK_DATA0); +} + +void WHILL::PacketParser::parseDataset1(Packet* packet){ + whill->accelerometer.x = (signed short)((packet->payload[1] << 8)+(packet->payload[2])); + whill->accelerometer.y = (signed short)((packet->payload[3] << 8)+(packet->payload[4])); + whill->accelerometer.z = (signed short)((packet->payload[5] << 8)+(packet->payload[6])); + + whill->gyro.x = (signed short)((packet->payload[7] << 8)+(packet->payload[8])); + whill->gyro.y = (signed short)((packet->payload[9] << 8)+(packet->payload[10])); + whill->gyro.z = (signed short)((packet->payload[11] << 8)+(packet->payload[12])); + + whill->joy.y = (int)(signed char)packet->payload[13]; + whill->joy.x = (int)(signed char)packet->payload[14]; + + whill->battery.level = (unsigned char)packet->payload[15]; + whill->battery.current = (signed short)((packet->payload[16] << 8)+(packet->payload[17])) * 2; //Unit : 2mA + + whill->right_motor.angle = (float)((signed short)((packet->payload[18] << 8)+(packet->payload[19])) * 0.001 ); + whill->left_motor.angle = (float)((signed short)((packet->payload[20] << 8)+(packet->payload[21])) * 0.001 ); + + whill->right_motor.speed = (signed short)((packet->payload[22] << 8)+(packet->payload[23])) * 4; + whill->left_motor.speed = (signed short)((packet->payload[24] << 8)+(packet->payload[25])) * 4; + + whill->power = packet->payload[26] == 0x00 ? false : true; + whill->speed_mode_indicator = packet->payload[27]; + + if(whill == NULL)return; + whill->fire_callback(CALLBACK_DATA1); +} diff --git a/src/whill/PacketReceiver.cpp b/src/whill/PacketReceiver.cpp new file mode 100644 index 0000000..d1df324 --- /dev/null +++ b/src/whill/PacketReceiver.cpp @@ -0,0 +1,92 @@ +/* +The MIT License (MIT) + +Copyright (c) 2018 WHILL, + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include "WHILL.h" + +int WHILL::PacketReceiver::push(unsigned char data){ + + if(!recording){ + if(data != WHILL::Packet::PROTOCOL_SIGN){ + index = 0; + return -1; + }else{ + recording = true; + } + } + + + buf[index] = data; + + if(index >= 1 && remaining_bytes() == 0){ + call_callback(); + index = 0; + recording = false; + return 0; + } + + index++; + return remaining_bytes(); +} + +int WHILL::PacketReceiver::remaining_bytes(){ + if(index == 0)return -1; + if(!recording)return -1; + + int length = 2 + buf[1]; // Protocl sign + length + [len](payload + cs) + + return length-(index+1); +} + +void WHILL::PacketReceiver::register_callback(void (*callback)()){ + this->obj = NULL; + this->method = NULL; + this->callback = callback; +} + +void WHILL::PacketReceiver::register_callback(PacketParser* obj,int(PacketParser::*method)(WHILL::Packet* packet)){ + this->obj = obj; + this->method = method; + this->callback = NULL; +} + + + +bool WHILL::PacketReceiver::call_callback(){ + + Packet packet; + packet.setRaw(buf,index+1); + + + if(callback != NULL){ + this->callback(); + return true; + } + + if(obj != NULL && method != NULL){ + (obj->*method)(&packet); + return true; + } + + return false; +} diff --git a/src/whill/WHILL.cpp b/src/whill/WHILL.cpp new file mode 100644 index 0000000..09ba3bd --- /dev/null +++ b/src/whill/WHILL.cpp @@ -0,0 +1,106 @@ +/* +The MIT License (MIT) + +Copyright (c) 2018 WHILL, + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include "WHILL.h" + +WHILL::WHILL(int (*read)(unsigned char *byte), int (*write)(unsigned char bytes[],int length)) +{ + this->read = read; + this->write = write; + + parser.setParent(this); + receiver.register_callback(&parser,&PacketParser::parsePacket); +} + + +// int WHILL::read(unsigned char* byte){ // Implementation of read interaface to WHILL +// if(serial == NULL) return -1; + +// int data = serial->read(); +// if(data == -1) return -1; //Nothing read + +// *byte = data; + +// return 1; +// } + +// int WHILL::write(unsigned char byte){ // Implementation of write interface to WHILL +// if(serial == NULL) return -1; +// serial->write(byte); + +// return 1; +// } + +void WHILL::begin(unsigned int interval){ + this->startSendingData1(interval); +} + +void WHILL::transferPacket(Packet* packet){ + unsigned char buffer[Packet::MAX_LENGTH] = {0}; + int size = packet->getRaw(buffer); + write(buffer,size); +} + +void WHILL::receivePacket(){ + unsigned char data; + while(read(&data) != -1){ + receiver.push(data); + } +} + +// void WHILL::keep_joy_delay(unsigned long ms){ +// while(ms > 0){ +// refresh(); +// if(ms%100 == 0){ +// this->setJoystick(virtual_joy.x,virtual_joy.y); +// } +// ms--; +// ::delay(1); +// } +// } + + +// void WHILL::delay(unsigned long ms){ +// while(ms > 0){ +// refresh(); +// ms--; +// ::delay(1); +// } +// } + + +void WHILL::refresh(){ + // Scan the data from interface + receivePacket(); +} + + +void WHILL::register_callback(Callback method,EVENT event){ + callback_functions[event] = method; +} + +void WHILL::fire_callback(EVENT event){ + if(callback_functions[event]==NULL)return; + callback_functions[event](this); +} \ No newline at end of file diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h new file mode 100644 index 0000000..717f538 --- /dev/null +++ b/src/whill/WHILL.h @@ -0,0 +1,199 @@ +/* +The MIT License (MIT) + +Copyright (c) 2018 WHILL, + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#ifndef __WHILL_H__ +#define __WHILL_H__ + +#include + + +class WHILL{ + + class Packet{ + + private: + + public: + const static unsigned char PROTOCOL_SIGN = 0xAF; + + const static int MAX_LENGTH = 35; + const static int MAX_PAYLOAD = MAX_LENGTH - 3; // protocol_sign,len,cs + + Packet(); + Packet(unsigned char payload[],int size); + + unsigned char getCalculatedCS(); + + unsigned char protocol_sign; + unsigned char len; + unsigned char payload[MAX_LENGTH]; + unsigned char cs; + + bool is_valid(); + + int rawLength(); + + bool setRaw(unsigned char* raw,int len); + int getRaw(unsigned char* raw); + void build(); + }; + + + + class PacketParser{ + + private: + WHILL* whill = NULL; + void parseDataset0(WHILL::Packet* packet); + void parseDataset1(WHILL::Packet* packet); + + public: + void setParent(WHILL* whill); + void setWHILLReceiver(WHILL* whill); + int parsePacket(Packet* packet); + + }; + + + class PacketReceiver{ + + private: + unsigned char buf[Packet::MAX_LENGTH] = {0}; + unsigned char index = 0; + bool recording = false; + + void(*callback)() = NULL; + bool call_callback(); + + PacketParser* obj = NULL; + int (PacketParser::*method)(WHILL::Packet* packet) = NULL; + + public: + int push(unsigned char data); + int remaining_bytes(); + void reset(); + void register_callback(void (*callback)()); + void register_callback(PacketParser* obj,int(PacketParser::*method)(WHILL::Packet* packet)); + }; + + + +private: + + // Custom transceiver + int (*read)(unsigned char *byte); + int (*write)(unsigned char bytes[],int length); + + void receivePacket(); + void transferPacket(Packet* packet); + + PacketReceiver receiver; + PacketParser parser; + + +public: + WHILL(int (*read)(unsigned char *byte), int (*write)(unsigned char bytes[], int length)); + void begin(unsigned int interval); + + //Callback + enum EVENT + { + CALLBACK_DATA0, + CALLBACK_DATA1, + CALLBACK_POWER_ON, + EVENT_SIZE + }; + typedef void (*Callback)(WHILL *); + Callback callback_functions[EVENT_SIZE] = {NULL}; + void register_callback(Callback method, EVENT event); + void fire_callback(EVENT event); + + void refresh(); + + void keep_joy_delay(unsigned long ms); + void delay(unsigned long ms); + + typedef struct + { + unsigned char forward_spped; + unsigned char forward_acceleration; + unsigned char forward_deceleration; + + unsigned char reverse_speed; + unsigned char reverse_acceleration; + unsigned char reverse_deceleration; + + unsigned char turn_speed; + unsigned char turn_acceleration; + unsigned char turn_deceleration; + } SpeedProfile; + + typedef struct + { + int x; + int y; + int z; + } Data3D; + + typedef struct + { + int x; + int y; + } Joy; + + typedef struct + { + unsigned char level; + signed long current; + } Battery; + + typedef struct + { + float angle; + int speed; + } Motor; + + Data3D accelerometer = {0}; + Data3D gyro = {0}; + Joy virtual_joy = {0}; + Joy joy = {0}; + Battery battery = {0}; + Motor left_motor = {0}; + Motor right_motor = {0}; + bool power = false; + int speed_mode_indicator = -1; + + //WHILL commands + void startSendingData0(unsigned int interval_ms, unsigned char speed_mode); + void startSendingData1(unsigned int interval_ms); + void stopSendingData(); + void setJoystick(int x, int y); + void setPower(bool power); + void setBatteryVoltaegeOut(bool out); + void setSpeedProfile(SpeedProfile * profile, unsigned char speed_mode); + +}; + + +#endif \ No newline at end of file diff --git a/src/whill/whill_commands.cpp b/src/whill/whill_commands.cpp new file mode 100644 index 0000000..71c9288 --- /dev/null +++ b/src/whill/whill_commands.cpp @@ -0,0 +1,101 @@ +/* +The MIT License (MIT) + +Copyright (c) 2018 WHILL, + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +#include "WHILL.h" + +void WHILL::startSendingData0(unsigned int interval_ms,unsigned char speed_mode){ + unsigned char payload[] = {0x00, //Start Sending Data + 0x00, //Data0 (Speed profiles) + (unsigned char)(interval_ms<<8 & 0xFF), + (unsigned char)(interval_ms<<0 & 0xFF), + speed_mode}; + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} + +void WHILL::startSendingData1(unsigned int interval_ms){ + unsigned char payload[] = {0x00, //Start Sending Data + 0x01, //Data1 (Sensors) + (unsigned char)(interval_ms<<8 & 0xFF), + (unsigned char)(interval_ms<<0 & 0xFF), + 0x00}; + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} + +void WHILL::stopSendingData(){ + unsigned char payload[] = {0x01}; // Stop Sending Data + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} + +void WHILL::setPower(bool power){ + unsigned char payload[] = {0x02, + (unsigned char)(power ? 0x01 : 0x00)}; + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} + +void WHILL::setJoystick(int x,int y){ + virtual_joy.x = x; + virtual_joy.y = y; + + unsigned char payload[] = {0x03, + 0x00, // Enable Host control + (unsigned char)(char)(y), + (unsigned char)(char)(x)}; + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} + + +void WHILL::setSpeedProfile(SpeedProfile* profile,unsigned char speed_mode){ + unsigned char payload[] = {0x04, + profile->forward_spped, + profile->forward_acceleration, + profile->forward_deceleration, + profile->reverse_speed, + profile->reverse_acceleration, + profile->reverse_deceleration, + profile->turn_speed, + profile->turn_acceleration, + profile->turn_deceleration}; + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} + + +void WHILL::setBatteryVoltaegeOut(bool enable){ + unsigned char payload[] = {0x05, + (unsigned char)(enable ? 0x01 : 0x00)}; + Packet packet(payload,sizeof(payload)); + packet.build(); + transferPacket(&packet); +} \ No newline at end of file diff --git a/src/whill_modelc/com_whill.cpp b/src/whill_modelc/com_whill.cpp deleted file mode 100644 index 26f2350..0000000 --- a/src/whill_modelc/com_whill.cpp +++ /dev/null @@ -1,291 +0,0 @@ -/* -MIT License - -Copyright (c) 2018 WHILL inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -/* - * Functions to communcate with WHILL via UART - * author : Kazumichi Shirai - */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "./uart.h" - -#include "ros/ros.h" - -#define NUM_ADD_COM (3) //Add "protocol sign". "length of data" and "checksum" -//Comand ID List -enum{ - START = 0, - STOP, - SET_POWER, - //SET_SEAT_MOVE, // removed on Model C - SET_JOYSTICK, - //SET_FORWARD, // removed on Model C - //SET_REVERSE, // removed on Model C - //SET_TURN, // removed on Model C - //SET_SPEED_DOWN, // removed on Model C - SET_SPEED_PROFILE, // added on Model C - SET_BATTERY_VOLTAGE_OUT, // added on Model C -}; - -#define USER_CTRL_DISABLE (0) -#define USER_CTRL_ENABLE (1) -#define ENABLE (1) - -int initializeComWHILL(int *fd,std::string port) -{ - if(initializeUART(fd,port) < 0){ - fprintf(stderr, "Can't initizalize UART to communicate with WHILL\n"); - return -1; - } - - return 1; -} - -void closeComWHILL(int fd) -{ - closeUART(fd); - close(fd); -} - -int sendWHILLCmd(int fd, char cmd_data[], int length) -{ - int i; - const int num_data = length + NUM_ADD_COM; - char data[num_data]; - int checksum = 0; - - data[0] = PROTOCOL_SIGN; - data[1] = length + 1; //length of cmd_data + 1(=checksum) - for(i=0;i> 8); - cmd[3] = (char)t; - cmd[4] = speed_mode; // added on Model C - - return sendWHILLCmd(fd, cmd, num_cmd); -} - -int sendStopSendingData(int fd) -{ - const int num_cmd = 1; - char cmd[num_cmd]; - - cmd[0] = STOP; - return sendWHILLCmd(fd, cmd, num_cmd); -} - -/* removed on Model C -int sendSeatMove(int fd, char direction) -{ - const int num_cmd = 3; - char cmd[num_cmd]; - - cmd[0] = SET_SEAT_MOVE; - cmd[1] = USER_CTRL_DISABLE; - cmd[2] = direction; - - return sendWHILLCmd(fd, cmd, num_cmd); -} -*/ - -int sendPowerOn(int fd) -{ - const int num_cmd = 2; - char cmd[num_cmd]; - - cmd[0] = SET_POWER; - cmd[1] = 1; - sendWHILLCmd(fd, cmd, num_cmd); - usleep(20000); - return sendWHILLCmd(fd, cmd, num_cmd); -} - -int sendPowerOff(int fd) -{ - const int num_cmd = 2; - char cmd[num_cmd]; - - cmd[0] = SET_POWER; - cmd[1] = 0; - - return sendWHILLCmd(fd, cmd, num_cmd); -} - -/* removed on Model C -int sendSetForward(int fd, char max_speed, char accel, char deccel) -{ - const int num_cmd = 4; - char cmd[num_cmd]; - - cmd[0] = SET_FORWARD; - cmd[1] = max_speed; - cmd[2] = accel; - cmd[3] = deccel; - - return sendWHILLCmd(fd, cmd, num_cmd); -} - -int sendSetReverse(int fd, char max_speed, char accel, char deccel) -{ - const int num_cmd = 4; - char cmd[num_cmd]; - - cmd[0] = SET_REVERSE; - cmd[1] = max_speed; - cmd[2] = accel; - cmd[3] = deccel; - - return sendWHILLCmd(fd, cmd, num_cmd); -} - -int sendSetTurn(int fd, char max_speed, char accel, char deccel) -{ - const int num_cmd = 4; - char cmd[num_cmd]; - - cmd[0] = SET_TURN; - cmd[1] = max_speed; - cmd[2] = accel; - cmd[3] = deccel; - - return sendWHILLCmd(fd, cmd, num_cmd); -} -*/ - -// added on Model C -int sendSetSpeed(int fd, char s1, char fm1, char fa1, char fd1, char rm1, char ra1, char rd1, char tm1, char ta1, char td1) -{ - const int num_cmd = 11; - char cmd[num_cmd]; - - cmd[0] = SET_SPEED_PROFILE; - cmd[1] = s1; - cmd[2] = fm1; - cmd[3] = fa1; - cmd[4] = fd1; - cmd[5] = rm1; - cmd[6] = ra1; - cmd[7] = rd1; - cmd[8] = tm1; - cmd[9] = ta1; - cmd[10] = td1; - - return sendWHILLCmd(fd, cmd, num_cmd); -} - -// added on Model C -int sendSetBatteryOut(int fd, char battery_out) -{ - const int num_cmd = 2; - char cmd[num_cmd]; - - cmd[0] = SET_BATTERY_VOLTAGE_OUT; - cmd[1] = battery_out; - - return sendWHILLCmd(fd, cmd, num_cmd); -} - diff --git a/src/whill_modelc/com_whill.h b/src/whill_modelc/com_whill.h deleted file mode 100644 index b56ff85..0000000 --- a/src/whill_modelc/com_whill.h +++ /dev/null @@ -1,58 +0,0 @@ -/* -MIT License - -Copyright (c) 2018 WHILL inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -/* - * Functions to communcate with WHILL via UART - * author : Kazumichi Shirai - */ -#include - -enum whill_const -{ - DATASET_NUM_ZERO = 0, - DATASET_NUM_ONE = 1, - POWERON_RESPONSE_DATA = 0x52, - SPEED_MODE = 0, - SEND_INTERVAL = 10, -}; - -int initializeComWHILL(int *fd, std::string port); -void closeComWHILL(int fd); -int sendJoystickStop(int fd); -//int sendSpeedDown(int fd, int rate); // removed on Model C -int recvDataWHILL(int fd, unsigned char recv_buf[]); -int sendJoystick(int fd, char fb, char lr); -int releaseJoystick(int fd); -//int sendStartSendingData(int fd, int t, char data_set_num); -int sendStartSendingData(int fd, int t, char data_set_num, char speed_mode); // modified on Model C -int sendStopSendingData(int fd); -//int sendSeatMove(int fd, char direction); // removed on Model C -int sendPowerOn(int fd); -int sendPowerOff(int fd); -//int sendSetForward(int fd, char max_speed, char accel, char deceel); // removed on Model C -//int sendSetReverse(int fd, char max_speed, char accel, char deccel); // removed on Model C -//int sendSetTurn(int fd, char max_speed, char accel, char deccel); // removed on Model C -int sendSetSpeed(int fd, char s1, char fm1, char fa1, char fd1, char rm1, char ra1, char rd1, char tm1, char ta1, char td1); // added on Model C -int sendSetBatteryOut(int fd, char battery_out); // added on Model C - diff --git a/src/whill_modelc/uart.cpp b/src/whill_modelc/uart.cpp deleted file mode 100644 index 9a24d08..0000000 --- a/src/whill_modelc/uart.cpp +++ /dev/null @@ -1,172 +0,0 @@ -/* -MIT License - -Copyright (c) 2018 WHILL inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -/* - * Functions to use UART - * author : Kazumichi Shirai - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "./uart.h" - -#include "ros/ros.h" - -//#define BAUDRATE B19200 //Model A -#define BAUDRATE B38400 //Model C - - -static struct termios oldtio; - -int sendCmdUART(int fd, char cmd[], int len) -{ - if(write(fd, cmd, len) != len) - { - fprintf(stderr, "fail to write command %02x \n", cmd[2]); - return -1; - } - return 1; -} - -int recvDataUART(int fd, unsigned char recv_buf[]) -{ - int size; - char prtcl; - char len, remain_len; - unsigned char tmp_recv_buf[128]; - int i, j,idx; - int retry = 3; - unsigned char check_sum = 0; - unsigned int error_count = 0; - - //** Recv Protocol Sign **// - error_count = 0; - do - { - size = read(fd, tmp_recv_buf, 1); - if(size < 0) return -1; - usleep(100); - if(error_count++ > 100) return -1; - }while((tmp_recv_buf[0] & 0xff) != PROTOCOL_SIGN); - - //fprintf(stdout, "protocl sign = %x\n", tmp_recv_buf[0]); - check_sum ^= tmp_recv_buf[0]; - - //** Recv Data length **// - error_count = 0; - do - { - size = read(fd, tmp_recv_buf, 1); - if(size < 0) return -1; - usleep(100); - if(error_count++ > 100) return -1; - } while (size == 0); - - //fprintf(stdout, "data length = %d\n", tmp_recv_buf[0]); - check_sum ^= tmp_recv_buf[0]; - len = tmp_recv_buf[0]; - remain_len =len; - idx = 0; - - //** Recv Data **// - error_count = 0; - while(remain_len > 0){ - size = read(fd, tmp_recv_buf, remain_len);//recv data - //fprintf(stdout, "remain_len = %d, size = %d\n", remain_len, size); - if(size < 0) - { - fprintf(stderr, "fail to read\n"); - return -1; - } - for(i=0; i 100) return -1; - } - - //** Check check_sum **// - for(i=0;i - -#define PROTOCOL_SIGN (0xAF) - -int sendCmdUART(int fd, char cmd[], int len); -int recvDataUART(int fd, unsigned char recv_buf[]); -int initializeUART(int *fd,std::string port); -void closeUART(int fd); diff --git a/src/whill_modelc_controller.cpp b/src/whill_modelc_controller.cpp deleted file mode 100644 index 3f5a4a6..0000000 --- a/src/whill_modelc_controller.cpp +++ /dev/null @@ -1,189 +0,0 @@ -/* -MIT License - -Copyright (c) 2018 WHILL inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#include "ros/ros.h" -#include "sensor_msgs/Joy.h" - -#include "ros_whill/srvSetSpeedProfile.h" -#include "ros_whill/srvSetPower.h" -#include "ros_whill/srvSetBatteryVoltageOut.h" -#include "whill_modelc/com_whill.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -int whill_fd; // file descriptor for UART to communicate with WHILL - -// Set Speed Profile -bool set_speed_profile_srv(ros_whill::srvSetSpeedProfile::Request &req, ros_whill::srvSetSpeedProfile::Response &res) -{ - - // value check - if(0 <= req.s1 && req.s1 <= 5 - && 8 <= req.fm1 && req.fm1 <= 60 - && 10 <= req.fa1 && req.fa1 <= 90 - && 10 <= req.fd1 && req.fd1 <= 160 - && 8 <= req.rm1 && req.rm1 <= 60 - && 10 <= req.ra1 && req.ra1 <= 90 - && 10 <= req.rd1 && req.rd1 <= 160 - && 8 <= req.tm1 && req.tm1 <= 60 - && 10 <= req.ta1 && req.ta1 <= 90 - && 10 <= req.td1 && req.td1 <= 160) - { - ROS_INFO("Speed profile is set"); - sendSetSpeed(whill_fd, req.s1, req.fm1, req.fa1, req.fd1, req.rm1, req.ra1, req.rd1, req.tm1, req.ta1, req.td1); - - res.result = 1; - return true; - } - else - { - ROS_WARN("wrong parameter is assigned."); - ROS_WARN("s1 must be assingned between 0 - 5"); - ROS_WARN("fm1 must be assingned between 8 - 60"); - ROS_WARN("fa1 must be assingned between 10 - 90"); - ROS_WARN("fd1 must be assingned between 10 - 160"); - ROS_WARN("rm1 must be assingned between 8 - 60"); - ROS_WARN("ra1 must be assingned between 10 - 90"); - ROS_WARN("rd1 must be assingned between 10 - 160"); - ROS_WARN("tm1 must be assingned between 8 - 60"); - ROS_WARN("ta1 must be assingned between 10 - 90"); - ROS_WARN("td1 must be assingned between 10 - 160"); - res.result = -1; - return false; - } -} - - -// Set Power -bool set_power_srv(ros_whill::srvSetPower::Request &req, ros_whill::srvSetPower::Response &res) -{ - // power off - if(req.p0 == 0) - { - sendPowerOff(whill_fd); - ROS_DEBUG("WHILL: Requested Power Off"); - res.result = 1; - return true; - } - // power on - else if (req.p0 == 1) - { - sendPowerOn(whill_fd); - usleep(2000); - - ROS_DEBUG("WHILL: Requested Power On"); - res.result = 1; - return true; - } - else - { - ROS_WARN("p0 must be assinged 0 or 1"); - res.result = -1; - return false; - } - -} - - -// Set Battery Voltage out -bool set_battery_voltage_out_srv(ros_whill::srvSetBatteryVoltageOut::Request &req, ros_whill::srvSetBatteryVoltageOut::Response &res) -{ - if(req.v0 == 0 || req.v0 == 1) - { - sendSetBatteryOut(whill_fd, req.v0); - if(req.v0 == 0) ROS_INFO("battery voltage out: disable"); - if(req.v0 == 1) ROS_INFO("battery voltage out: enable"); - res.result = 1; - return true; - } - else - { - ROS_INFO("v0 must be assigned 0 or 1"); - res.result = -1; - return false; - } - -} - - -// Set Joystick - -void whillSetJoyMsgCallback(const sensor_msgs::Joy::ConstPtr& joy) -{ - int joy_side = -joy->axes[0] * 100.0f; - int joy_front = joy->axes[1] * 100.0f; - - // value check - if(joy_front < -100) joy_front = -100; - if(joy_front > 100) joy_front = 100; - if(joy_side < -100) joy_side = -100; - if(joy_side > 100) joy_side = 100; - - sendJoystick(whill_fd, joy_front, joy_side); - -} - - -int main(int argc, char **argv) -{ - // ROS setup - ros::init(argc, argv, "whill_modelc_controller"); - ros::NodeHandle nh; - - ros::NodeHandle nh_("~"); - - std::string serialport = "/dev/ttyUSB0"; - nh_.getParam("serialport", serialport); - - // Services - ros::ServiceServer set_speed_profile_svr = nh.advertiseService("set_speed_profile_srv", set_speed_profile_srv); - ros::ServiceServer set_power_svr = nh.advertiseService("set_power_srv", set_power_srv); - ros::ServiceServer set_battery_voltage_out_svr = nh.advertiseService("set_battery_voltage_out_srv", set_battery_voltage_out_srv); - - // Subscribers - ros::Subscriber whill_setjoy_sub = nh.subscribe("/whill/controller/joy", 100, whillSetJoyMsgCallback); - - initializeComWHILL(&whill_fd,serialport); - - ros::spin(); - - closeComWHILL(whill_fd); - - return 0; -} - - From 40798b88ca65ad1cb9985df8859335a26df98969 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 26 Apr 2019 16:58:43 +0900 Subject: [PATCH 02/41] Delete useless files --- src/whill_modelc_publisher.cpp | 414 -------------------------------- srv/srvSetBatteryVoltageOut.srv | 3 - srv/srvSetPower.srv | 3 - 3 files changed, 420 deletions(-) delete mode 100644 src/whill_modelc_publisher.cpp delete mode 100644 srv/srvSetBatteryVoltageOut.srv delete mode 100644 srv/srvSetPower.srv diff --git a/src/whill_modelc_publisher.cpp b/src/whill_modelc_publisher.cpp deleted file mode 100644 index 1f8c37c..0000000 --- a/src/whill_modelc_publisher.cpp +++ /dev/null @@ -1,414 +0,0 @@ -/* -MIT License - -Copyright (c) 2018 WHILL inc. - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. -*/ - -#include "ros/ros.h" - - -#include "sensor_msgs/Joy.h" -#include "sensor_msgs/JointState.h" -#include "sensor_msgs/Imu.h" -#include "sensor_msgs/BatteryState.h" -#include "nav_msgs/Odometry.h" -#include "geometry_msgs/TransformStamped.h" -#include "tf/transform_broadcaster.h" -#include "std_srvs/Empty.h" - -#include "ros_whill/msgWhillModelC.h" -#include "ros_whill/msgWhillSpeedProfile.h" -#include "whill_modelc/com_whill.h" - -#include "./odom.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#define MAX_EVENTS (10) - -#define DATASET_LEN_V01 (30) -#define DATASET_LEN_V02 (31) // Provision for future release. - -#define ACC_CONST (0.000122) -#define GYR_CONST (0.004375) -#define MANGLE_CONST (0.001) -#define MSPEED_CONST (0.004) - -Odometry odom; - -int registerFdToEpoll(struct epoll_event *ev, int epollfd, int fd) -{ - /* register socket to epoll */ - ev->events = EPOLLIN; - ev->data.fd = fd; - if (epoll_ctl(epollfd, EPOLL_CTL_ADD, fd, ev) == -1) { - ROS_INFO("fail epoll_ctl\n"); - return -1; - } - return 1; -} - -float calc_16bit_signed_data(char d1, char d2) -{ - float d = 0; - d = float(((d1 & 0xff) << 8) | (d2 & 0xff)); - if(d > pow(2, 15)){ - d = d - pow(2, 16); - } - - return d; -} - -float calc_8bit_signed_data(char d1) -{ - float d = 0; - d = float(d1 & 0xff); - if(d > pow(2, 7)){ - d = d - pow(2, 8); - } - - return d; -} - -double calc_rad_diff(double past,double current){ - double diff = past - current; - if(past * current < 0 && fabs(diff) > M_PI){ // Crossed +PI/-PI[rad] border - diff = M_PI-fabs(past) + (M_PI-fabs(current)); // - to + - if(past > 0 && current < 0){ // Case of + to - - diff = -diff; - } - } - return diff; -} - -unsigned int calc_time_diff(unsigned int past,unsigned int current){ - int diff = current - past; - if(abs(diff) >= 100){ - diff = (201 - past) + current; - } - return (unsigned int)diff; -} - -bool clearOdom(std_srvs::Empty::Request &req,std_srvs::Empty::Response &res){ - ROS_INFO("Clear Odometry"); - odom.reset(); - return true; -} - - -int main(int argc, char **argv) -{ - // ROS setup - ros::init(argc, argv, "whill_modelc_publisher"); - ros::NodeHandle nh; - ros::Publisher whill_modelc_pub = nh.advertise("whill_modelc_msg", 100); - - ros::Publisher whill_modelc_joy = nh.advertise("/whill/states/joy", 100); - ros::Publisher whill_modelc_jointState = nh.advertise("/whill/states/jointState", 100); - ros::Publisher whill_modelc_imu = nh.advertise("/whill/states/imu", 100); - ros::Publisher whill_modelc_battery = nh.advertise("/whill/states/batteryState", 100); - ros::Publisher whill_modelc_odom = nh.advertise("/whill/odom", 100); - - ros::ServiceServer service = nh.advertiseService("/whill/odom/clear", &clearOdom); - - ros::Publisher whill_speed_profile_pub = nh.advertise("whill_speed_profile_msg", 10); - - tf::TransformBroadcaster odom_broadcaster_; - - ros::NodeHandle nh_("~"); // For parameters - double wheel_radius = 0.135; - nh_.getParam("wheel_radius", wheel_radius); - - std::string serialport = "/dev/ttyUSB0"; - nh_.getParam("serialport", serialport); - - // Node Param - int send_interval = SEND_INTERVAL; - nh_.getParam("send_interval", send_interval); - if(send_interval < 10){ - ROS_WARN("Too short interval. Set interval > 10"); - nh.setParam("/whill_modelc_publisher/send_interval", 10); - send_interval = 10; - } - ROS_INFO("param: send_interval=%d", send_interval); - - // WHILL setup - struct epoll_event ev; - struct epoll_event events[MAX_EVENTS]; - int epollfd, nfds; - int whill_fd; // file descriptor for UART to communicate with WHILL - unsigned char recv_buf[128]; - int len, idx; - int i; - - initializeComWHILL(&whill_fd,serialport); - if((epollfd = epoll_create1(0)) < 0) - { - ROS_INFO("can't creare epoll\n"); - return -1; - } - - // register Fd to epoll - registerFdToEpoll(&ev, epollfd, whill_fd); - - // Power On WHILL - sendPowerOn(whill_fd); - usleep(2000); - - // Send StartSendingData command: dataset 0 for all speed mode - i = 0; - while(i < 6) - { - sendStopSendingData(whill_fd); - usleep(2000); - do - { //flush old buffer - len = recvDataWHILL(whill_fd, recv_buf); - } while (len > 0); - sendStartSendingData(whill_fd, 25, DATASET_NUM_ZERO, i); - usleep(2000); - do - { - len = recvDataWHILL(whill_fd, recv_buf); - } while (len == 0); - - if(recv_buf[0] == DATASET_NUM_ZERO && len == 12) - { - ros_whill::msgWhillSpeedProfile msg_sp; - msg_sp.s1 = int(recv_buf[1] & 0xff); - msg_sp.fm1 = int(recv_buf[2] & 0xff); - msg_sp.fa1 = int(recv_buf[3] & 0xff); - msg_sp.fd1 = int(recv_buf[4] & 0xff); - msg_sp.rm1 = int(recv_buf[5] & 0xff); - msg_sp.ra1 = int(recv_buf[6] & 0xff); - msg_sp.rd1 = int(recv_buf[7] & 0xff); - msg_sp.tm1 = int(recv_buf[8] & 0xff); - msg_sp.ta1 = int(recv_buf[9] & 0xff); - msg_sp.td1 = int(recv_buf[10] & 0xff); - whill_speed_profile_pub.publish(msg_sp); - ROS_INFO("Speed profile %ld is published", msg_sp.s1); - i++; - } - } - - // send StopSendingData command - sendStopSendingData(whill_fd); - usleep(2000); - // Send StartSendingData command: dataset 1 - sendStartSendingData(whill_fd, send_interval, DATASET_NUM_ONE, SPEED_MODE); - usleep(2000); - - // loop - int msg_cnt = 0; - ROS_INFO("Start WHILL message reporting"); - while(ros::ok()) - { - //ROS_INFO("WHILL message %d is published", msg_cnt ++); - ros_whill::msgWhillModelC msg; - sensor_msgs::Joy joy; - sensor_msgs::JointState jointState; - sensor_msgs::Imu imu; - sensor_msgs::BatteryState batteryState; - nav_msgs::Odometry odomMsg; - - // WHILL data - // Process epoll - nfds = epoll_wait(epollfd, events, MAX_EVENTS, -1); - if (nfds == -1) { - ROS_INFO("Error : epoll wait"); - break; - } - - for (i = 0; i < nfds; i++) { - // Receive Data From WHILL - if(events[i].data.fd == whill_fd) { - len = recvDataWHILL(whill_fd, recv_buf); - ROS_DEBUG("recv_buf[0] = 0x%x, len = %d", recv_buf[0], len); - if(recv_buf[0] == POWERON_RESPONSE_DATA && len == 2) - { - ROS_INFO("WHILL: Successfully powered on."); - } - else if(recv_buf[0] == DATASET_NUM_ONE && (len == DATASET_LEN_V01 || len == DATASET_LEN_V02)) - { - ros::Time currentTime = ros::Time::now(); - joy.header.stamp = currentTime; - jointState.header.stamp = currentTime; - imu.header.stamp = currentTime; - - msg.acc_x = calc_16bit_signed_data(recv_buf[1], recv_buf[2]) * ACC_CONST; - msg.acc_y = calc_16bit_signed_data(recv_buf[3], recv_buf[4]) * ACC_CONST; - msg.acc_z = calc_16bit_signed_data(recv_buf[5], recv_buf[6]) * ACC_CONST; - - msg.gyr_x = calc_16bit_signed_data(recv_buf[7], recv_buf[8]) * GYR_CONST; - msg.gyr_y = calc_16bit_signed_data(recv_buf[9], recv_buf[10]) * GYR_CONST; - msg.gyr_z = calc_16bit_signed_data(recv_buf[11], recv_buf[12]) * GYR_CONST; - - msg.joy_front = (int)calc_8bit_signed_data(recv_buf[13]); - msg.joy_side = (int)calc_8bit_signed_data(recv_buf[14]); - - msg.battery_power = int(recv_buf[15] & 0xff); - msg.battery_current = calc_16bit_signed_data(recv_buf[16], recv_buf[17]); //TODO -> fixed point - - msg.right_motor_angle = calc_16bit_signed_data(recv_buf[18], recv_buf[19]) * MANGLE_CONST; - msg.left_motor_angle = calc_16bit_signed_data(recv_buf[20], recv_buf[21]) * MANGLE_CONST; - msg.right_motor_speed = calc_16bit_signed_data(recv_buf[22], recv_buf[23]) * MSPEED_CONST; - msg.left_motor_speed = calc_16bit_signed_data(recv_buf[24], recv_buf[25]) * MSPEED_CONST; - - msg.power_on = int(recv_buf[26] & 0xff); - - msg.speed_mode_indicator = int(recv_buf[27] & 0xff); - - - unsigned int time_diff_ms = 0; - unsigned int time_ms = 0; - static unsigned int past_time_ms = 0; - static ros::Time pastTime = ros::Time(0); - if(len == DATASET_LEN_V02) - { - time_ms = static_cast(recv_buf[29] & 0xff); - time_diff_ms = calc_time_diff(past_time_ms,time_ms); - past_time_ms = time_ms; - ROS_DEBUG("New style time count (Experimental). Interval=%d", time_diff_ms); - } - else - { - time_diff_ms = static_cast((currentTime - pastTime).toSec() * 1000); - pastTime = currentTime; - ROS_DEBUG("Conventional style time count. Interval=%d", time_diff_ms); - } - - - // IMU message - imu.header.frame_id = "imu"; - - imu.orientation_covariance[0] = -1; // Orientation is unknown - - imu.angular_velocity.x = msg.gyr_x / 180 * M_PI; // deg per sec to rad/s - imu.angular_velocity.y = msg.gyr_y / 180 * M_PI; // deg per sec to rad/s - imu.angular_velocity.z = msg.gyr_z / 180 * M_PI; // deg per sec to rad/s - - imu.linear_acceleration.x = msg.acc_x * 9.80665; // G to m/ss - imu.linear_acceleration.y = msg.acc_y * 9.80665; // G to m/ss - imu.linear_acceleration.z = msg.acc_z * 9.80665; // G to m/ss - - // Battery State - - batteryState.voltage = 25.2; //[V] - batteryState.current = msg.battery_current / 1000.0f; // mA -> A - batteryState.charge = std::numeric_limits::quiet_NaN(); - batteryState.capacity = std::numeric_limits::quiet_NaN(); - batteryState.design_capacity = 10.04;//[Ah] - batteryState.percentage = msg.battery_power / 100.0f; // Percentage - batteryState.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; - batteryState.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; - batteryState.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_LION; - batteryState.present = true; - batteryState.location = "0"; - - - // Joy message - - joy.axes.resize(2); - joy.axes[0] = -msg.joy_side / 100.0f; //X - joy.axes[1] = msg.joy_front / 100.0f; //Y - - - // JointState message - jointState.name.resize(2); - jointState.position.resize(2); - jointState.velocity.resize(2); - - jointState.name[0] = "leftWheel"; - jointState.position[0] = msg.left_motor_angle; //Rad - - static double past[2] = {0.0f,0.0f}; - - if(time_diff_ms != 0)jointState.velocity[0] = calc_rad_diff(past[0],jointState.position[0]) / (double(time_diff_ms) / 1000.0f); - else jointState.velocity[0] = 0; - - past[0] = jointState.position[0]; - - jointState.name[1] = "rightWheel"; - jointState.position[1] = msg.right_motor_angle; //Rad - - if(time_diff_ms != 0)jointState.velocity[1] = calc_rad_diff(past[1],jointState.position[1]) / (double(time_diff_ms) / 1000.0f); - else jointState.velocity[0] = 0; - past[1] = jointState.position[1]; - - - odom.update(jointState,time_diff_ms/1000.0f); - - - msg.error = int(recv_buf[28] & 0xff); - if(msg.error != 0) - { - ROS_WARN("WHILL sends error message. error id: %d", msg.error); - } - - // publish - whill_modelc_joy.publish(joy); - whill_modelc_jointState.publish(jointState); - whill_modelc_imu.publish(imu); - whill_modelc_battery.publish(batteryState); - - // Publish Odometry - geometry_msgs::TransformStamped odom_trans = odom.getROSTransformStamped(); - odom_trans.header.stamp = currentTime; - odom_trans.header.frame_id = "odom"; - odom_trans.child_frame_id = "base_footprint"; - odom_broadcaster_.sendTransform(odom_trans); - - nav_msgs::Odometry odom_msg = odom.getROSOdometry(); - odom_msg.header.stamp = currentTime; - odom_msg.header.frame_id = "odom"; - odom_msg.child_frame_id = "base_footprint"; - whill_modelc_odom.publish(odom_msg); - } - } - } - - ros::spinOnce(); - - - } - - // send StopSendingData command - ROS_INFO("Request Stop Sending Data."); - sendStopSendingData(whill_fd); - usleep(1000); - ROS_INFO("Closing Serial Port."); - closeComWHILL(whill_fd); - return 0; -} - - diff --git a/srv/srvSetBatteryVoltageOut.srv b/srv/srvSetBatteryVoltageOut.srv deleted file mode 100644 index f0e6881..0000000 --- a/srv/srvSetBatteryVoltageOut.srv +++ /dev/null @@ -1,3 +0,0 @@ -int64 v0 ---- -int64 result diff --git a/srv/srvSetPower.srv b/srv/srvSetPower.srv deleted file mode 100644 index 742fb2f..0000000 --- a/srv/srvSetPower.srv +++ /dev/null @@ -1,3 +0,0 @@ -int64 p0 ---- -int64 result From 90558f1242908d21357b0765cc12f3949fb4f573 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Sat, 27 Apr 2019 01:48:53 +0900 Subject: [PATCH 03/41] Porting from WHILL Arduino SDK --- package.xml | 3 +- src/ros_whill.cpp | 87 ++++++++++++++++++++++++++++++++++----------- src/whill/WHILL.cpp | 16 ++++++--- src/whill/WHILL.h | 9 ++--- 4 files changed, 85 insertions(+), 30 deletions(-) diff --git a/package.xml b/package.xml index 2f782ba..2880707 100644 --- a/package.xml +++ b/package.xml @@ -53,13 +53,14 @@ roscpp rospy std_msgs - roscpp + roscpp rospy std_msgs roscpp rospy std_msgs + serial diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index a409afa..9bc3fd8 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -24,6 +24,7 @@ SOFTWARE. #include #include +#include #include "ros/ros.h" #include "sensor_msgs/Joy.h" @@ -32,13 +33,61 @@ SOFTWARE. #include "sensor_msgs/BatteryState.h" #include "nav_msgs/Odometry.h" -#include "whill/WHILL.h" +#include "whill/WHILL.h" +#include "serial/serial.h" // wwjwood/Serial (ros-melodic-serial) // #include "./includes/subscriber.hpp" // #include "./includes/services.hpp" -#include "serial/serial.h" + +// +// UART Interface +// +serial::Serial *ser = NULL; + +int serialRead(std::vector &data) +{ + if (ser) + { + return ser->read(data, 30); // How many bytes read in one time. + } + return 0; +} + +int serialWrite(std::vector &data) +{ + if (ser) + { + return ser->write(data); + } + return 0; +} + + +// +// WHILL +// +WHILL *whill = NULL; + +void callback_data1(WHILL *caller) +{ + // This function is called when receive Joy/Accelerometer/Gyro,etc. + ROS_INFO("Updated"); + ROS_INFO("%d",caller->joy.x); + ROS_INFO("%d",caller->joy.y); +} + +void callback_powered_on(WHILL *caller) +{ + // This function is called when powered on via setPower() + ROS_INFO("power_on"); +} + + +// +// Main +// int main(int argc, char **argv) { // ROS setup @@ -62,11 +111,21 @@ int main(int argc, char **argv) } ROS_INFO("param: send_interval=%d", send_interval); + std::string port = "/dev/ttyUSB0"; + unsigned long baud = 38400; + + ser = new serial::Serial(port, baud, serial::Timeout::simpleTimeout(0)); + + whill = new WHILL(serialRead, serialWrite); + whill->register_callback(callback_data1, WHILL::EVENT::CALLBACK_DATA1); + whill->register_callback(callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); + whill->begin(10); + // // Services // ros::ServiceServer set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); // //ros::ServiceServer service = nh.advertiseService("odom/clear", &clearOdom); - // // Subscribers + // // SubscriberstransferPacket // ros::Subscriber control_joystick_subscriber = nh.subscribe("controller/joy", 100, control_joystick_callback); // Defined in subscriber.cpp // // Publishers @@ -86,28 +145,14 @@ int main(int argc, char **argv) // Publishers } - usleep(100); - - std::string port = "/dev/ttyUSB0"; - unsigned long baud = 38400; - serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(100)); - ros::AsyncSpinner spinner(1); spinner.start(); - ros::Rate rate(1); - - // - // // cmd[0] = START; - // cmd[1] = data_set_num; - // cmd[2] = (char)(t >> 8); - // cmd[3] = (char)t; - // cmd[4] = speed_mode; // added on Model C - /// - //sendStartSendingData(my_serial, send_interval, DATASET_NUM_ONE, SPEED_MODE); - usleep(2000); + ros::Rate rate(10); while (ros::ok()) { + //whill->refresh(); + whill->setJoystick(50, 0); // std::vector received; // //size_t length = my_serial.read(received,20); // std::cout << length << ", String read: " << std::endl; @@ -116,7 +161,9 @@ int main(int argc, char **argv) // printf("%02x,", (unsigned int)*i); // } //ROS_INFO("sleep()"); + rate.sleep(); + } spinner.stop(); diff --git a/src/whill/WHILL.cpp b/src/whill/WHILL.cpp index 09ba3bd..cc5a4db 100644 --- a/src/whill/WHILL.cpp +++ b/src/whill/WHILL.cpp @@ -24,7 +24,7 @@ THE SOFTWARE. #include "WHILL.h" -WHILL::WHILL(int (*read)(unsigned char *byte), int (*write)(unsigned char bytes[],int length)) +WHILL::WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)) { this->read = read; this->write = write; @@ -59,14 +59,20 @@ void WHILL::begin(unsigned int interval){ void WHILL::transferPacket(Packet* packet){ unsigned char buffer[Packet::MAX_LENGTH] = {0}; int size = packet->getRaw(buffer); - write(buffer,size); + std::vectordata(buffer, buffer + size * sizeof(buffer[0])); + write(data); } void WHILL::receivePacket(){ - unsigned char data; - while(read(&data) != -1){ - receiver.push(data); + std::vector data; + + if(read(data) > 0){ + size_t size = data.size(); + for (size_t i = 0; i < size;i++){ + receiver.push(data[i]); + } } + } // void WHILL::keep_joy_delay(unsigned long ms){ diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index 717f538..33c3068 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -25,7 +25,8 @@ THE SOFTWARE. #ifndef __WHILL_H__ #define __WHILL_H__ -#include +#include +#include class WHILL{ @@ -102,8 +103,8 @@ class WHILL{ private: // Custom transceiver - int (*read)(unsigned char *byte); - int (*write)(unsigned char bytes[],int length); + int (*read)(std::vector &data); // Returns how many bytes read actually + int (*write)(std::vector &data); // Returns how many bytes wrote actually void receivePacket(); void transferPacket(Packet* packet); @@ -113,7 +114,7 @@ class WHILL{ public: - WHILL(int (*read)(unsigned char *byte), int (*write)(unsigned char bytes[], int length)); + WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)); void begin(unsigned int interval); //Callback From 79ca3a8f42f200dbf0ad0866b9a33999b1f49a47 Mon Sep 17 00:00:00 2001 From: seiya0412 Date: Sun, 5 May 2019 00:52:01 +0900 Subject: [PATCH 04/41] Update CMakeLists.txt --- src/whill/CMakeLists.txt | 66 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 62 insertions(+), 4 deletions(-) diff --git a/src/whill/CMakeLists.txt b/src/whill/CMakeLists.txt index f6f24eb..d8d093a 100644 --- a/src/whill/CMakeLists.txt +++ b/src/whill/CMakeLists.txt @@ -1,8 +1,66 @@ cmake_minimum_required(VERSION 2.8) -add_library(lib_whill STATIC - Packet.cpp - PacketParser.cpp +project(libwhill) + +if (NOT CMAKE_BUILD_TYPE) + message(STATUS "No build type selected, default to Release") + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) +endif() + +if (NOT CMAKE_BUILD_TARGET) + message(STATUS "No target type selected, default to both shared and static libraries") + set(CMAKE_BUILD_TARGET "Both" CACHE STRING "" FORCE) +endif() + +option(BUILD_EXAMPLE "Build example program" OFF) +option(BUILD_TEST "Build test program" OFF) + +set(${PROJECT_NAME}_VERSION_MAJOR 0) +set(${PROJECT_NAME}_VERSION_MINOR 0) +set(${PROJECT_NAME}_VERSION_PATCH 0) +set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_VERSION_MAJOR}.${${PROJECT_NAME}_VERSION_MINOR}.${${PROJECT_NAME}_VERION_PATCH}) + +set(${PROJECT_NAME}_DESCRIPTION "A cross-platform library for WHILL Model CR") +set(${PROJECT_NAME}_URL "https://github.com/WHILL/ros_whill") + +# Try to find serial communication library https://github.com/wjwwood/serial +find_package(serial) +if (NOT serial_FOUND) +#[WIP] build serial package here +endif() + +set(SOURCES + Packet.cpp + PacketParser.cpp PacketReceiver.cpp whill_commands.cpp WHILL.cpp -) \ No newline at end of file +) + +if(${CMAKE_BUILD_TARGET} MATCHES "Shared") + set(BUILD_WHILL_SHARED TRUE) +elseif(${CMAKE_BUILD_TARGET} MATCHES "Static") + set(BUILD_WHILL_STATIC TRUE) +elseif(${CMAKE_BUILD_TARGET} MATCHES "Both") + set(BUILD_WHILL_SHARED TRUE) + set(BUILD_WHILL_STATIC TRUE) +else() + message(FATAL_ERROR "Invalid build type ${CMAKE_BUILD_TYPE}") +endif() + +if(BULD_WHILL_SHARED) + add_library(whill SHARED ${SOURCES}) + set_property(TARGET whill PROPERTY VERSION ${${PROJECT_NAME}_VERSON}) + set_property(TARGET whill PROPERTY SOVERSION ${${PROJECT_NAME}_VERSON_MAJOR}) + list(APPEND WHILL_TARGETS whill) +endif() + +if(BULD_WHILL_STATIC) + add_library(whill_static STATIC ${SOURCES}) + set_target_properties(whill_static PROPERTIES OUTPUT_NAME whill) + list(APPEND WHILL_TARGETS whill_static) +endif() + + + + + From 19f6bc1a3aa576db6d29231ea958d208eac029d3 Mon Sep 17 00:00:00 2001 From: seiya0412 Date: Sun, 5 May 2019 00:54:03 +0900 Subject: [PATCH 05/41] Revert "Update CMakeLists.txt" This reverts commit 79ca3a8f42f200dbf0ad0866b9a33999b1f49a47. --- src/whill/CMakeLists.txt | 66 +++------------------------------------- 1 file changed, 4 insertions(+), 62 deletions(-) diff --git a/src/whill/CMakeLists.txt b/src/whill/CMakeLists.txt index d8d093a..f6f24eb 100644 --- a/src/whill/CMakeLists.txt +++ b/src/whill/CMakeLists.txt @@ -1,66 +1,8 @@ cmake_minimum_required(VERSION 2.8) -project(libwhill) - -if (NOT CMAKE_BUILD_TYPE) - message(STATUS "No build type selected, default to Release") - set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) -endif() - -if (NOT CMAKE_BUILD_TARGET) - message(STATUS "No target type selected, default to both shared and static libraries") - set(CMAKE_BUILD_TARGET "Both" CACHE STRING "" FORCE) -endif() - -option(BUILD_EXAMPLE "Build example program" OFF) -option(BUILD_TEST "Build test program" OFF) - -set(${PROJECT_NAME}_VERSION_MAJOR 0) -set(${PROJECT_NAME}_VERSION_MINOR 0) -set(${PROJECT_NAME}_VERSION_PATCH 0) -set(${PROJECT_NAME}_VERSION ${${PROJECT_NAME}_VERSION_MAJOR}.${${PROJECT_NAME}_VERSION_MINOR}.${${PROJECT_NAME}_VERION_PATCH}) - -set(${PROJECT_NAME}_DESCRIPTION "A cross-platform library for WHILL Model CR") -set(${PROJECT_NAME}_URL "https://github.com/WHILL/ros_whill") - -# Try to find serial communication library https://github.com/wjwwood/serial -find_package(serial) -if (NOT serial_FOUND) -#[WIP] build serial package here -endif() - -set(SOURCES - Packet.cpp - PacketParser.cpp +add_library(lib_whill STATIC + Packet.cpp + PacketParser.cpp PacketReceiver.cpp whill_commands.cpp WHILL.cpp -) - -if(${CMAKE_BUILD_TARGET} MATCHES "Shared") - set(BUILD_WHILL_SHARED TRUE) -elseif(${CMAKE_BUILD_TARGET} MATCHES "Static") - set(BUILD_WHILL_STATIC TRUE) -elseif(${CMAKE_BUILD_TARGET} MATCHES "Both") - set(BUILD_WHILL_SHARED TRUE) - set(BUILD_WHILL_STATIC TRUE) -else() - message(FATAL_ERROR "Invalid build type ${CMAKE_BUILD_TYPE}") -endif() - -if(BULD_WHILL_SHARED) - add_library(whill SHARED ${SOURCES}) - set_property(TARGET whill PROPERTY VERSION ${${PROJECT_NAME}_VERSON}) - set_property(TARGET whill PROPERTY SOVERSION ${${PROJECT_NAME}_VERSON_MAJOR}) - list(APPEND WHILL_TARGETS whill) -endif() - -if(BULD_WHILL_STATIC) - add_library(whill_static STATIC ${SOURCES}) - set_target_properties(whill_static PROPERTIES OUTPUT_NAME whill) - list(APPEND WHILL_TARGETS whill_static) -endif() - - - - - +) \ No newline at end of file From 025edb69fd92edebaed2330ddc9c2836a29f874f Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Tue, 7 May 2019 16:50:25 +0900 Subject: [PATCH 06/41] Set UART timeout time separatery and check serial status in read/write function --- src/ros_whill.cpp | 26 +++++++++----------------- 1 file changed, 9 insertions(+), 17 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 9bc3fd8..cd4ebe1 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -39,8 +39,6 @@ SOFTWARE. // #include "./includes/subscriber.hpp" // #include "./includes/services.hpp" - - // // UART Interface // @@ -48,7 +46,7 @@ serial::Serial *ser = NULL; int serialRead(std::vector &data) { - if (ser) + if (ser && ser->isOpen()) { return ser->read(data, 30); // How many bytes read in one time. } @@ -57,7 +55,7 @@ int serialRead(std::vector &data) int serialWrite(std::vector &data) { - if (ser) + if (ser && ser->isOpen()) { return ser->write(data); } @@ -111,10 +109,13 @@ int main(int argc, char **argv) } ROS_INFO("param: send_interval=%d", send_interval); - std::string port = "/dev/ttyUSB0"; + std::string port = "/dev/tty.whill"; unsigned long baud = 38400; - ser = new serial::Serial(port, baud, serial::Timeout::simpleTimeout(0)); + serial::Timeout timeout = serial::Timeout::simpleTimeout(0); + timeout.write_timeout_multiplier = 5; + + ser = new serial::Serial(port, baud, timeout); whill = new WHILL(serialRead, serialWrite); whill->register_callback(callback_data1, WHILL::EVENT::CALLBACK_DATA1); @@ -147,23 +148,14 @@ int main(int argc, char **argv) ros::AsyncSpinner spinner(1); spinner.start(); - ros::Rate rate(10); + ros::Rate rate(100); while (ros::ok()) { - //whill->refresh(); + whill->refresh(); whill->setJoystick(50, 0); - // std::vector received; - // //size_t length = my_serial.read(received,20); - // std::cout << length << ", String read: " << std::endl; - // for (std::vector::iterator i = begin(received); i != end(received); ++i) - // { - // printf("%02x,", (unsigned int)*i); - // } - //ROS_INFO("sleep()"); rate.sleep(); - } spinner.stop(); From 36cceecefe169e798ec5e69564b7ac38b3698ef8 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 8 May 2019 12:26:12 +0900 Subject: [PATCH 07/41] Implement Experimental commands --- src/ros_whill.cpp | 9 +- src/whill/PacketParser.cpp | 20 ++- src/whill/WHILL.cpp | 65 +++----- src/whill/WHILL.h | 279 ++++++++++++++++++----------------- src/whill/whill_commands.cpp | 39 ++++- 5 files changed, 221 insertions(+), 191 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index cd4ebe1..f16c227 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -74,6 +74,7 @@ void callback_data1(WHILL *caller) ROS_INFO("Updated"); ROS_INFO("%d",caller->joy.x); ROS_INFO("%d",caller->joy.y); + ROS_INFO("interval:%d",caller->_interval); } void callback_powered_on(WHILL *caller) @@ -104,7 +105,7 @@ int main(int argc, char **argv) if (send_interval < 10) { ROS_WARN("Too short interval. Set interval > 10"); - nh.setParam("/whill_modelc_publisher/send_interval", 10); + nh.setParam("send_interval", 10); send_interval = 10; } ROS_INFO("param: send_interval=%d", send_interval); @@ -116,11 +117,12 @@ int main(int argc, char **argv) timeout.write_timeout_multiplier = 5; ser = new serial::Serial(port, baud, timeout); + ser->flush(); whill = new WHILL(serialRead, serialWrite); whill->register_callback(callback_data1, WHILL::EVENT::CALLBACK_DATA1); whill->register_callback(callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); - whill->begin(10); + whill->begin(10); // ms // // Services // ros::ServiceServer set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); @@ -153,8 +155,7 @@ int main(int argc, char **argv) while (ros::ok()) { whill->refresh(); - whill->setJoystick(50, 0); - + whill->setSpeed(0.0f,0.5f); rate.sleep(); } diff --git a/src/whill/PacketParser.cpp b/src/whill/PacketParser.cpp index 1649f81..010554d 100644 --- a/src/whill/PacketParser.cpp +++ b/src/whill/PacketParser.cpp @@ -21,6 +21,7 @@ LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +#include "stdio.h" #include "WHILL.h" @@ -75,9 +76,9 @@ void WHILL::PacketParser::parseDataset1(Packet* packet){ whill->battery.level = (unsigned char)packet->payload[15]; whill->battery.current = (signed short)((packet->payload[16] << 8)+(packet->payload[17])) * 2; //Unit : 2mA - - whill->right_motor.angle = (float)((signed short)((packet->payload[18] << 8)+(packet->payload[19])) * 0.001 ); - whill->left_motor.angle = (float)((signed short)((packet->payload[20] << 8)+(packet->payload[21])) * 0.001 ); + + whill->right_motor.angle = (float)((signed short)((packet->payload[18] << 8) + (packet->payload[19])) * 0.001); + whill->left_motor.angle = (float)((signed short)((packet->payload[20] << 8) + (packet->payload[21])) * 0.001); whill->right_motor.speed = (signed short)((packet->payload[22] << 8)+(packet->payload[23])) * 4; whill->left_motor.speed = (signed short)((packet->payload[24] << 8)+(packet->payload[25])) * 4; @@ -85,6 +86,19 @@ void WHILL::PacketParser::parseDataset1(Packet* packet){ whill->power = packet->payload[26] == 0x00 ? false : true; whill->speed_mode_indicator = packet->payload[27]; + // Experimental, Set interval comming from Motor Controller + if (packet->rawLength() == WHILL::Packet::DATASET1_LEN_V02){ + whill->_interval = 1; + uint8_t time_ms = packet->payload[29]; + whill->_interval = WHILL::calc_time_diff(whill->past_time_ms,time_ms); + whill->past_time_ms = time_ms; + } + else + { + whill->past_time_ms = 0; + whill->_interval = -1; + } + if(whill == NULL)return; whill->fire_callback(CALLBACK_DATA1); } diff --git a/src/whill/WHILL.cpp b/src/whill/WHILL.cpp index cc5a4db..4ef0942 100644 --- a/src/whill/WHILL.cpp +++ b/src/whill/WHILL.cpp @@ -24,35 +24,19 @@ THE SOFTWARE. #include "WHILL.h" +#include + WHILL::WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)) { - this->read = read; - this->write = write; + this->read = read; // Register UART read interface pointer + this->write = write; // Register UART write interface pointer parser.setParent(this); receiver.register_callback(&parser,&PacketParser::parsePacket); } - -// int WHILL::read(unsigned char* byte){ // Implementation of read interaface to WHILL -// if(serial == NULL) return -1; - -// int data = serial->read(); -// if(data == -1) return -1; //Nothing read - -// *byte = data; - -// return 1; -// } - -// int WHILL::write(unsigned char byte){ // Implementation of write interface to WHILL -// if(serial == NULL) return -1; -// serial->write(byte); - -// return 1; -// } - -void WHILL::begin(unsigned int interval){ +void WHILL::begin(unsigned int interval) +{ this->startSendingData1(interval); } @@ -65,7 +49,7 @@ void WHILL::transferPacket(Packet* packet){ void WHILL::receivePacket(){ std::vector data; - + if(read(data) > 0){ size_t size = data.size(); for (size_t i = 0; i < size;i++){ @@ -75,27 +59,6 @@ void WHILL::receivePacket(){ } -// void WHILL::keep_joy_delay(unsigned long ms){ -// while(ms > 0){ -// refresh(); -// if(ms%100 == 0){ -// this->setJoystick(virtual_joy.x,virtual_joy.y); -// } -// ms--; -// ::delay(1); -// } -// } - - -// void WHILL::delay(unsigned long ms){ -// while(ms > 0){ -// refresh(); -// ms--; -// ::delay(1); -// } -// } - - void WHILL::refresh(){ // Scan the data from interface receivePacket(); @@ -109,4 +72,16 @@ void WHILL::register_callback(Callback method,EVENT event){ void WHILL::fire_callback(EVENT event){ if(callback_functions[event]==NULL)return; callback_functions[event](this); -} \ No newline at end of file +} + +// Experimental +uint8_t WHILL::calc_time_diff(uint8_t past, uint8_t current) +{ + // Counts up 0 to 201. If counter exceeds 201, goes to 0. + int diff = current - past; + if (abs(diff) >= 100) // Half + { + diff = (201 - past) + current; + } + return (uint8_t)diff; +} diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index 33c3068..a30a114 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -1,3 +1,4 @@ + /* The MIT License (MIT) @@ -28,173 +29,177 @@ THE SOFTWARE. #include #include +class WHILL +{ -class WHILL{ - - class Packet{ + class Packet + { - private: + private: + public: + const static unsigned char PROTOCOL_SIGN = 0xAF; - public: - const static unsigned char PROTOCOL_SIGN = 0xAF; + const static int MAX_LENGTH = 35; + const static int MAX_PAYLOAD = MAX_LENGTH - 3; // protocol_sign,len,cs - const static int MAX_LENGTH = 35; - const static int MAX_PAYLOAD = MAX_LENGTH - 3; // protocol_sign,len,cs + const static int DATASET1_LEN_V01 = 32; // Stable + const static int DATASET1_LEN_V02 = 33; // Experimental, Additional timing data from Motor Controller - Packet(); - Packet(unsigned char payload[],int size); + Packet(); + Packet(unsigned char payload[], int size); - unsigned char getCalculatedCS(); + unsigned char getCalculatedCS(); - unsigned char protocol_sign; - unsigned char len; - unsigned char payload[MAX_LENGTH]; - unsigned char cs; + unsigned char protocol_sign; + unsigned char len; + unsigned char payload[MAX_LENGTH]; + unsigned char cs; - bool is_valid(); + bool is_valid(); + int rawLength(); - int rawLength(); - - bool setRaw(unsigned char* raw,int len); - int getRaw(unsigned char* raw); - void build(); + bool setRaw(unsigned char *raw, int len); + int getRaw(unsigned char *raw); + void build(); }; + class PacketParser + { + private: + WHILL *whill = NULL; + void parseDataset0(WHILL::Packet *packet); + void parseDataset1(WHILL::Packet *packet); - class PacketParser{ - - private: - WHILL* whill = NULL; - void parseDataset0(WHILL::Packet* packet); - void parseDataset1(WHILL::Packet* packet); - - public: - void setParent(WHILL* whill); - void setWHILLReceiver(WHILL* whill); - int parsePacket(Packet* packet); - + public: + void setParent(WHILL *whill); + void setWHILLReceiver(WHILL *whill); + int parsePacket(Packet *packet); }; + class PacketReceiver + { - class PacketReceiver{ - - private: - unsigned char buf[Packet::MAX_LENGTH] = {0}; - unsigned char index = 0; - bool recording = false; + private: + unsigned char buf[Packet::MAX_LENGTH] = {0}; + unsigned char index = 0; + bool recording = false; - void(*callback)() = NULL; - bool call_callback(); + void (*callback)() = NULL; + bool call_callback(); - PacketParser* obj = NULL; - int (PacketParser::*method)(WHILL::Packet* packet) = NULL; + PacketParser *obj = NULL; + int (PacketParser::*method)(WHILL::Packet *packet) = NULL; - public: - int push(unsigned char data); - int remaining_bytes(); - void reset(); - void register_callback(void (*callback)()); - void register_callback(PacketParser* obj,int(PacketParser::*method)(WHILL::Packet* packet)); + public: + int push(unsigned char data); + int remaining_bytes(); + void reset(); + void register_callback(void (*callback)()); + void register_callback(PacketParser *obj, int (PacketParser::*method)(WHILL::Packet *packet)); }; - - private: - // Custom transceiver - int (*read)(std::vector &data); // Returns how many bytes read actually - int (*write)(std::vector &data); // Returns how many bytes wrote actually + int (*read)(std::vector &data); // Returns how many bytes read actually + int (*write)(std::vector &data); // Returns how many bytes wrote actually void receivePacket(); - void transferPacket(Packet* packet); + void transferPacket(Packet *packet); PacketReceiver receiver; - PacketParser parser; + PacketParser parser; + // Experimantal + uint8_t past_time_ms = 0; + static uint8_t calc_time_diff(uint8_t past, uint8_t current); public: - WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)); - void begin(unsigned int interval); - - //Callback - enum EVENT - { - CALLBACK_DATA0, - CALLBACK_DATA1, - CALLBACK_POWER_ON, - EVENT_SIZE - }; - typedef void (*Callback)(WHILL *); - Callback callback_functions[EVENT_SIZE] = {NULL}; - void register_callback(Callback method, EVENT event); - void fire_callback(EVENT event); - - void refresh(); - - void keep_joy_delay(unsigned long ms); - void delay(unsigned long ms); - - typedef struct - { - unsigned char forward_spped; - unsigned char forward_acceleration; - unsigned char forward_deceleration; - - unsigned char reverse_speed; - unsigned char reverse_acceleration; - unsigned char reverse_deceleration; - - unsigned char turn_speed; - unsigned char turn_acceleration; - unsigned char turn_deceleration; - } SpeedProfile; - - typedef struct - { - int x; - int y; - int z; - } Data3D; - - typedef struct - { - int x; - int y; - } Joy; - - typedef struct - { - unsigned char level; - signed long current; - } Battery; - - typedef struct - { - float angle; - int speed; - } Motor; - - Data3D accelerometer = {0}; - Data3D gyro = {0}; - Joy virtual_joy = {0}; - Joy joy = {0}; - Battery battery = {0}; - Motor left_motor = {0}; - Motor right_motor = {0}; - bool power = false; - int speed_mode_indicator = -1; - - //WHILL commands - void startSendingData0(unsigned int interval_ms, unsigned char speed_mode); - void startSendingData1(unsigned int interval_ms); - void stopSendingData(); - void setJoystick(int x, int y); - void setPower(bool power); - void setBatteryVoltaegeOut(bool out); - void setSpeedProfile(SpeedProfile * profile, unsigned char speed_mode); - + WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)); + void begin(unsigned int interval); + + //Callback + enum EVENT + { + CALLBACK_DATA0, + CALLBACK_DATA1, + CALLBACK_POWER_ON, + EVENT_SIZE + }; + typedef void (*Callback)(WHILL *); + Callback callback_functions[EVENT_SIZE] = {NULL}; + void register_callback(Callback method, EVENT event); + void fire_callback(EVENT event); + + void refresh(); + + void keep_joy_delay(unsigned long ms); + void delay(unsigned long ms); + + typedef struct + { + unsigned char forward_spped; + unsigned char forward_acceleration; + unsigned char forward_deceleration; + + unsigned char reverse_speed; + unsigned char reverse_acceleration; + unsigned char reverse_deceleration; + + unsigned char turn_speed; + unsigned char turn_acceleration; + unsigned char turn_deceleration; + } SpeedProfile; + + typedef struct + { + int x; + int y; + int z; + } Data3D; + + typedef struct + { + int x; + int y; + } Joy; + + typedef struct + { + unsigned char level; + signed long current; + } Battery; + + typedef struct + { + float angle; + int speed; + } Motor; + + Data3D accelerometer = {0}; + Data3D gyro = {0}; + Joy virtual_joy = {0}; + Joy joy = {0}; + Battery battery = {0}; + Motor left_motor = {0}; + Motor right_motor = {0}; + bool power = false; + int speed_mode_indicator = -1; + + // Experimental + int32_t _interval = -1; // ms + + //WHILL commands + void startSendingData0(unsigned int interval_ms, unsigned char speed_mode); + void startSendingData1(unsigned int interval_ms); + void stopSendingData(); + void setJoystick(int x, int y); + void setPower(bool power); + void setBatteryVoltaegeOut(bool out); + void setSpeedProfile(SpeedProfile * profile, unsigned char speed_mode); + + // Experimental + void setSpeed(float linear, float angular); // Linear:[m/s], Angular:[rad/s] }; - #endif \ No newline at end of file diff --git a/src/whill/whill_commands.cpp b/src/whill/whill_commands.cpp index 71c9288..7bffed2 100644 --- a/src/whill/whill_commands.cpp +++ b/src/whill/whill_commands.cpp @@ -1,7 +1,7 @@ /* The MIT License (MIT) -Copyright (c) 2018 WHILL, +Copyright (c) 2018-2019 WHILL, Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -98,4 +98,39 @@ void WHILL::setBatteryVoltaegeOut(bool enable){ Packet packet(payload,sizeof(payload)); packet.build(); transferPacket(&packet); -} \ No newline at end of file +} + +// Experimental, Speed control without Jerk control but only Acceleration cotnrol. +void WHILL::setSpeed(float linear, float angular) +{ + int x, y; + + const float front_max = 6.0; // [km/h] + const float spin_max = 3.5; // [km/h] + const float back_max = 2.0; // [km/h] + const float tread_width = 0.248; // [m] + + float desire_front_kmh = linear * 3.6; // [m/s] to [km/h] + float desire_spin_spd = -tread_width * angular * 3.6 * 2; // [rad/s] to [km/h] + + if (linear >= 0) + { + //forward + y = desire_front_kmh / front_max * 100; + } + else + { + //back + y = desire_front_kmh / back_max * 100; + } + + x = desire_spin_spd / spin_max * 100; + + unsigned char payload[] = {0x08, // Experimental Command, Control without Jerk + 0x00, // Beta, SET_ACCEL_JOY = 0x08 + (unsigned char)(char)(y), + (unsigned char)(char)(x)}; + Packet packet(payload, sizeof(payload)); + packet.build(); + transferPacket(&packet); +} From 4bf63c363b75729e169a5431495f298217d93b9b Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 8 May 2019 17:18:44 +0900 Subject: [PATCH 08/41] Delete legacy msg file --- msg/msgWhillModelC.msg | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 msg/msgWhillModelC.msg diff --git a/msg/msgWhillModelC.msg b/msg/msgWhillModelC.msg deleted file mode 100644 index 81644ec..0000000 --- a/msg/msgWhillModelC.msg +++ /dev/null @@ -1,17 +0,0 @@ -float32 acc_x -float32 acc_y -float32 acc_z -float32 gyr_x -float32 gyr_y -float32 gyr_z -int32 joy_front -int32 joy_side -int32 battery_power -float32 battery_current -float32 right_motor_angle -float32 left_motor_angle -float32 right_motor_speed -float32 left_motor_speed -int32 power_on -int32 speed_mode_indicator -int32 error From 54f23e6193169bde5fe2e7349143810bb2b7b143 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 8 May 2019 17:19:10 +0900 Subject: [PATCH 09/41] Fix accel control command --- src/whill/whill_commands.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/whill/whill_commands.cpp b/src/whill/whill_commands.cpp index 7bffed2..56ad21b 100644 --- a/src/whill/whill_commands.cpp +++ b/src/whill/whill_commands.cpp @@ -117,17 +117,21 @@ void WHILL::setSpeed(float linear, float angular) { //forward y = desire_front_kmh / front_max * 100; + y = y > 100 ? 100 : y; } else { //back y = desire_front_kmh / back_max * 100; + y = y < -100 ? -100 : y; } x = desire_spin_spd / spin_max * 100; + x = x > 100 ? 100 : x; + x = x < -100 ? -100 : x; - unsigned char payload[] = {0x08, // Experimental Command, Control without Jerk - 0x00, // Beta, SET_ACCEL_JOY = 0x08 + unsigned char payload[] = {0x08, // Experimental Command, Control with Low Jerk, Almost Const-Accel control + 0x00, (unsigned char)(char)(y), (unsigned char)(char)(x)}; Packet packet(payload, sizeof(payload)); From a443896f4e15fbf54ba5ee19b8b61b638b7b73a8 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 8 May 2019 17:21:13 +0900 Subject: [PATCH 10/41] Add Subscriber --- src/ros_whill.cpp | 113 +++++++++++++++++++++++++++++++++------------- 1 file changed, 81 insertions(+), 32 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index f16c227..ce27440 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -34,15 +34,62 @@ SOFTWARE. #include "nav_msgs/Odometry.h" #include "whill/WHILL.h" -#include "serial/serial.h" // wwjwood/Serial (ros-melodic-serial) +#include "serial/serial.h" // wjwood/Serial (ros-melodic-serial) // #include "./includes/subscriber.hpp" // #include "./includes/services.hpp" +WHILL *whill = nullptr; + +// +// ROS Objects +// + +// Publishers +ros::Publisher joystick_state_publisher; +ros::Publisher jointstate_publisher; +ros::Publisher imu_publisher; +ros::Publisher battery_state_publisher; +ros::Publisher odom_publisher; + + +// +// ROS Callbacks +// +void ros_joystick_callback(const sensor_msgs::Joy::ConstPtr &joy) +{ + // Transform [-1.0,1.0] to [-100,100] + int joy_x = -joy->axes[0] * 100.0f; + int joy_y = joy->axes[1] * 100.0f; + + // value check + if (joy_y < -100) + joy_y = -100; + if (joy_y > 100) + joy_y = 100; + if (joy_x < -100) + joy_x = -100; + if (joy_x > 100) + joy_x = 100; + + if (whill) + { + whill->setJoystick(joy_x, joy_y); + } +} + +void ros_cmd_vel_callback(const geometry_msgs::Twist::ConstPtr &cmd_vel) +{ + if (whill) + { + whill->setSpeed(cmd_vel->linear.x, cmd_vel->angular.z); + } +} + // // UART Interface // -serial::Serial *ser = NULL; +serial::Serial *ser = nullptr; int serialRead(std::vector &data) { @@ -62,13 +109,10 @@ int serialWrite(std::vector &data) return 0; } - // // WHILL // -WHILL *whill = NULL; - -void callback_data1(WHILL *caller) +void whill_callback_data1(WHILL *caller) { // This function is called when receive Joy/Accelerometer/Gyro,etc. ROS_INFO("Updated"); @@ -77,7 +121,7 @@ void callback_data1(WHILL *caller) ROS_INFO("interval:%d",caller->_interval); } -void callback_powered_on(WHILL *caller) +void whill_callback_powered_on(WHILL *caller) { // This function is called when powered on via setPower() ROS_INFO("power_on"); @@ -99,6 +143,33 @@ int main(int argc, char **argv) bool activate_experimental; nh.param("activate_experimental", activate_experimental, false); + // Services + //set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); + //clear_odom_service = nh.advertiseService("odom/clear", &clearOdom); + + // Subscriber + ros::Subscriber joystick_subscriber = nh.subscribe("controller/joy", 100, ros_joystick_callback); + ros::Subscriber twist_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); + + // // Publishers + joystick_state_publisher = nh.advertise("states/joy", 100); + jointstate_publisher = nh.advertise("states/jointState", 100); + imu_publisher = nh.advertise("states/imu", 100); + battery_state_publisher = nh.advertise("states/batteryState", 100); + odom_publisher = nh.advertise("odom", 100); + + if (activate_experimental) + { + //Services + + //Subscribers + ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); + + // Publishers + } + + + // Node Param int send_interval = 10; nh.getParam("send_interval", send_interval); @@ -120,33 +191,11 @@ int main(int argc, char **argv) ser->flush(); whill = new WHILL(serialRead, serialWrite); - whill->register_callback(callback_data1, WHILL::EVENT::CALLBACK_DATA1); - whill->register_callback(callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); + whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); + whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); whill->begin(10); // ms - // // Services - // ros::ServiceServer set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); - // //ros::ServiceServer service = nh.advertiseService("odom/clear", &clearOdom); - // // SubscriberstransferPacket - // ros::Subscriber control_joystick_subscriber = nh.subscribe("controller/joy", 100, control_joystick_callback); // Defined in subscriber.cpp - - // // Publishers - // ros::Publisher joystick_state_publisher = nh.advertise("states/joy", 100); - // ros::Publisher jointstate_publisher = nh.advertise("states/jointState", 100); - // ros::Publisher imu_publisher = nh.advertise("states/imu", 100); - // ros::Publisher battery_state_publisher = nh.advertise("states/batteryState", 100); - // ros::Publisher odom_publisher = nh.advertise("odom", 100); - - if (activate_experimental) - { - //Services - - //Subscribers - //ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, control_cmd_vel_callback); - - // Publishers - } ros::AsyncSpinner spinner(1); spinner.start(); @@ -155,7 +204,7 @@ int main(int argc, char **argv) while (ros::ok()) { whill->refresh(); - whill->setSpeed(0.0f,0.5f); + //whill->setSpeed(0.0f,0.5f); rate.sleep(); } From dd4b228787afd331bd02c3f2f9ed5b87c45a1ff7 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 17:59:53 +0900 Subject: [PATCH 11/41] Publish States --- CMakeLists.txt | 2 +- src/odom.cpp | 58 ++++++------- src/odom.h | 34 ++++++++ src/ros_whill.cpp | 156 +++++++++++++++++++++++++++++------ src/utils/rotation_tools.cpp | 42 ++++++++++ src/utils/rotation_tools.h | 31 +++++++ 6 files changed, 268 insertions(+), 55 deletions(-) create mode 100644 src/odom.h create mode 100644 src/utils/rotation_tools.cpp create mode 100644 src/utils/rotation_tools.h diff --git a/CMakeLists.txt b/CMakeLists.txt index b5d341c..e279502 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -136,7 +136,7 @@ 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/ros_whill_node.cpp) -add_executable(ros_whill src/ros_whill.cpp) +add_executable(ros_whill src/ros_whill.cpp src/utils/rotation_tools.cpp src/odom.cpp) ## Rename C++ executable without prefix diff --git a/src/odom.cpp b/src/odom.cpp index b3d89d9..2ad6330 100644 --- a/src/odom.cpp +++ b/src/odom.cpp @@ -1,18 +1,14 @@ /* MIT License - -Copyright (c) 2018 WHILL inc. - +Copyright (c) 2018-2019 WHILL inc. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: - The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. - THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE @@ -45,28 +41,31 @@ SOFTWARE. #include #include +const float base_link_height = 0.1325; - -Odometry::Odometry(){ +Odometry::Odometry() +{ pose.x = pose.y = pose.theta = 0.0; velocity.x = velocity.y = velocity.theta = 0.0; } - -long double Odometry::confineRadian(long double rad){ - if(rad >= M_PI){ +long double Odometry::confineRadian(long double rad) +{ + if (rad >= M_PI) + { rad -= 2.0 * M_PI; } - if(rad <= -M_PI){ + if (rad <= -M_PI) + { rad += 2.0 * M_PI; } return rad; } - -void Odometry::update(sensor_msgs::JointState jointState,double dt) +void Odometry::update(sensor_msgs::JointState jointState, double dt) { - if(dt == 0)return; + if (dt == 0) + return; double angle_vel_r = jointState.velocity[1]; double angle_vel_l = -jointState.velocity[0]; @@ -74,10 +73,9 @@ void Odometry::update(sensor_msgs::JointState jointState,double dt) long double vr = angle_vel_r * wheel_radius_; long double vl = angle_vel_l * wheel_radius_; - long double delta_L = (vr + vl) / 2.0; + long double delta_L = (vr + vl) / 2.0; long double delta_theta = (vr - vl) / (2.0 * wheel_tread_); - pose.x += delta_L * dt * cosl(pose.theta + delta_theta * dt / 2.0); pose.y += delta_L * dt * sinl(pose.theta + delta_theta * dt / 2.0); @@ -91,22 +89,25 @@ void Odometry::update(sensor_msgs::JointState jointState,double dt) return; } - -void Odometry::reset(){ - Space2D poseZero = {0,0,0}; +void Odometry::reset() +{ + Space2D poseZero = {0, 0, 0}; set(poseZero); velocity = poseZero; } -void Odometry::set(Space2D pose){ +void Odometry::set(Space2D pose) +{ this->pose = pose; } -Odometry::Space2D Odometry::getOdom(){ +Odometry::Space2D Odometry::getOdom() +{ return pose; } -nav_msgs::Odometry Odometry::getROSOdometry(){ +nav_msgs::Odometry Odometry::getROSOdometry() +{ nav_msgs::Odometry odom; geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0, 0, pose.theta); @@ -114,7 +115,7 @@ nav_msgs::Odometry Odometry::getROSOdometry(){ // position odom.pose.pose.position.x = pose.x; odom.pose.pose.position.y = pose.y; - odom.pose.pose.position.z = 0.0; + odom.pose.pose.position.z = base_link_height; odom.pose.pose.orientation = odom_quat; //velocity @@ -128,9 +129,8 @@ nav_msgs::Odometry Odometry::getROSOdometry(){ return odom; } - - -geometry_msgs::TransformStamped Odometry::getROSTransformStamped(){ +geometry_msgs::TransformStamped Odometry::getROSTransformStamped() +{ geometry_msgs::TransformStamped odom_trans; @@ -138,8 +138,8 @@ geometry_msgs::TransformStamped Odometry::getROSTransformStamped(){ odom_trans.transform.translation.x = pose.x; odom_trans.transform.translation.y = pose.y; - odom_trans.transform.translation.z = 0.0; + odom_trans.transform.translation.z = base_link_height; odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pose.theta); - return odom_trans; -} + return odom_trans; +} \ No newline at end of file diff --git a/src/odom.h b/src/odom.h new file mode 100644 index 0000000..947983a --- /dev/null +++ b/src/odom.h @@ -0,0 +1,34 @@ +#pragma once + +#include "sensor_msgs/JointState.h" +#include "nav_msgs/Odometry.h" +#include "geometry_msgs/TransformStamped.h" + +class Odometry +{ +private: + long double confineRadian(long double rad); + + typedef struct + { + long double x; + long double y; + long double theta; + } Space2D; + + static constexpr double wheel_radius_ = 0.1325; + static constexpr double wheel_tread_ = 0.248; + + Space2D pose; + Space2D velocity; + +public: + Odometry(); + void update(sensor_msgs::JointState joint, double dt); + void set(Space2D pose); + void reset(); + + nav_msgs::Odometry getROSOdometry(); + geometry_msgs::TransformStamped getROSTransformStamped(); + Space2D getOdom(); +}; diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index ce27440..3c17421 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -33,25 +33,38 @@ SOFTWARE. #include "sensor_msgs/BatteryState.h" #include "nav_msgs/Odometry.h" +#include "tf/transform_broadcaster.h" + #include "whill/WHILL.h" #include "serial/serial.h" // wjwood/Serial (ros-melodic-serial) +#include "utils/rotation_tools.h" +#include "odom.h" + // #include "./includes/subscriber.hpp" // #include "./includes/services.hpp" WHILL *whill = nullptr; +Odometry odom; + + +// Global Parameters +int interval = 0; // WHILL Data frequency +bool publish_tf = true; // Enable publishing Odometry TF // // ROS Objects // // Publishers -ros::Publisher joystick_state_publisher; -ros::Publisher jointstate_publisher; -ros::Publisher imu_publisher; -ros::Publisher battery_state_publisher; -ros::Publisher odom_publisher; +ros::Publisher ros_joystick_state_publisher; +ros::Publisher ros_jointstate_publisher; +ros::Publisher ros_imu_publisher; +ros::Publisher ros_battery_state_publisher; +ros::Publisher ros_odom_publisher; +// TF Broadcaster +tf::TransformBroadcaster *odom_broadcaster; // // ROS Callbacks @@ -114,11 +127,108 @@ int serialWrite(std::vector &data) // void whill_callback_data1(WHILL *caller) { + ros::Time currentTime = ros::Time::now(); + // This function is called when receive Joy/Accelerometer/Gyro,etc. ROS_INFO("Updated"); ROS_INFO("%d",caller->joy.x); - ROS_INFO("%d",caller->joy.y); - ROS_INFO("interval:%d",caller->_interval); + ROS_INFO("%d", caller->joy.y); + ROS_INFO("interval:%d", caller->_interval); + + // Joy + sensor_msgs::Joy joy; + joy.header.stamp = currentTime; + joy.axes.resize(2); + joy.axes[0] = -caller->joy.x / 100.0f; //X + joy.axes[1] = caller->joy.y / 100.0f; //Y + ros_joystick_state_publisher.publish(joy); + + // IMU + sensor_msgs::Imu imu; + imu.header.frame_id = "imu"; + + imu.orientation_covariance[0] = -1; // Orientation is unknown + + imu.angular_velocity.x = caller->gyro.x / 180 * M_PI; // deg per sec to rad/s + imu.angular_velocity.y = caller->gyro.y / 180 * M_PI; // deg per sec to rad/s + imu.angular_velocity.z = caller->gyro.z / 180 * M_PI; // deg per sec to rad/s + + imu.linear_acceleration.x = caller->accelerometer.x * 9.80665; // G to m/ss + imu.linear_acceleration.y = caller->accelerometer.y * 9.80665; // G to m/ssnav_msgs::Odometry odom_msg = odom.getROSOdometry(); + imu.linear_acceleration.z = caller->accelerometer.z * 9.80665; // G to m/ss + ros_imu_publisher.publish(imu); + + // Battery + sensor_msgs::BatteryState batteryState; + batteryState.voltage = 25.2; //[V] Spec voltage, since raw voltage is not provided. + batteryState.current = -caller->battery.current / 1000.0f; // mA -> A + batteryState.charge = std::numeric_limits::quiet_NaN(); + batteryState.design_capacity = 10.04; //[Ah] + batteryState.percentage = caller->battery.level / 100.0f; // Percentage + batteryState.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; + batteryState.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; + batteryState.power_supply_technology = sensor_msgs::BatteryState::POWER_SUPPLY_TECHNOLOGY_LION; + batteryState.present = true; + batteryState.location = "0"; + ros_battery_state_publisher.publish(batteryState); + + // JointState + sensor_msgs::JointState jointState; + jointState.name.resize(2); + jointState.position.resize(2); + jointState.velocity.resize(2); + + jointState.name[0] = "leftWheel"; + jointState.position[0] = caller->left_motor.angle; //Rad + jointState.name[1] = "rightWheel"; + jointState.position[1] = caller->right_motor.angle; //Rad + + static double joint_past[2] = {0.0f, 0.0f}; + if(caller->_interval == -1){ + // Standard, Constant specified time intervel + jointState.velocity[0] = rad_diff(joint_past[0], jointState.position[0]) / (double(interval) / 1000.0f); // Rad/sec + jointState.velocity[1] = rad_diff(joint_past[1], jointState.position[1]) / (double(interval) / 1000.0f); // Rad/sec + }else if(caller->_interval == 0){ + // Experimental, Motor Control Disabled (= Brake Locked) + jointState.velocity[0] = 0.0f; + jointState.velocity[1] = 0.0f; + }else{ + // Experimental, Under motor controlling + jointState.velocity[0] = rad_diff(joint_past[0], jointState.position[0]) / (double(caller->_interval) / 1000.0f); // Rad/sec + jointState.velocity[1] = rad_diff(joint_past[1], jointState.position[1]) / (double(caller->_interval) / 1000.0f); // Rad/sec + } + joint_past[0] = jointState.position[0]; + joint_past[1] = jointState.position[1]; + + ros_jointstate_publisher.publish(jointState); + + + // Odometory + if(caller->_interval == -1){ + // Standard + odom.update(jointState, interval / 1000.0f); + }else if(caller->_interval >= 0){ + // Experimental + odom.update(jointState, caller->_interval / 1000.0f); + } + + nav_msgs::Odometry odom_msg = odom.getROSOdometry(); + odom_msg.header.stamp = currentTime; + odom_msg.header.frame_id = "odom"; + odom_msg.child_frame_id = "base_link"; + ros_odom_publisher.publish(odom_msg); + + // Odometory TF + if (publish_tf) + { + geometry_msgs::TransformStamped odom_trans = odom.getROSTransformStamped(); + odom_trans.header.stamp = currentTime; + odom_trans.header.frame_id = "odom"; + odom_trans.child_frame_id = "base_link"; + if (odom_broadcaster){ + odom_broadcaster->sendTransform(odom_trans); + } + } } void whill_callback_powered_on(WHILL *caller) @@ -152,34 +262,31 @@ int main(int argc, char **argv) ros::Subscriber twist_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); // // Publishers - joystick_state_publisher = nh.advertise("states/joy", 100); - jointstate_publisher = nh.advertise("states/jointState", 100); - imu_publisher = nh.advertise("states/imu", 100); - battery_state_publisher = nh.advertise("states/batteryState", 100); - odom_publisher = nh.advertise("odom", 100); - + ros_joystick_state_publisher = nh.advertise("states/joy", 100); + ros_jointstate_publisher = nh.advertise("states/jointState", 100); + ros_imu_publisher = nh.advertise("states/imu", 100); + ros_battery_state_publisher = nh.advertise("states/batteryState", 100); + ros_odom_publisher = nh.advertise("odom", 100); + + // TF Broadcaster + odom_broadcaster = new tf::TransformBroadcaster; + if (activate_experimental) { - //Services - - //Subscribers ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); - - // Publishers } // Node Param - int send_interval = 10; - nh.getParam("send_interval", send_interval); - if (send_interval < 10) + nh.getParam("send_interval", interval); + if (interval < 10) { ROS_WARN("Too short interval. Set interval > 10"); nh.setParam("send_interval", 10); - send_interval = 10; + interval = 10; } - ROS_INFO("param: send_interval=%d", send_interval); + ROS_INFO("param: send_interval=%d", interval); std::string port = "/dev/tty.whill"; unsigned long baud = 38400; @@ -193,7 +300,7 @@ int main(int argc, char **argv) whill = new WHILL(serialRead, serialWrite); whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); - whill->begin(10); // ms + whill->begin(20); // ms @@ -204,7 +311,6 @@ int main(int argc, char **argv) while (ros::ok()) { whill->refresh(); - //whill->setSpeed(0.0f,0.5f); rate.sleep(); } diff --git a/src/utils/rotation_tools.cpp b/src/utils/rotation_tools.cpp new file mode 100644 index 0000000..cf1fdbc --- /dev/null +++ b/src/utils/rotation_tools.cpp @@ -0,0 +1,42 @@ +/* +MIT License + +Copyright (c) 2019 WHILL inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* Author: Hikaru Sugiura */ + +#include +#include "rotation_tools.h" + +double rad_diff(double past, double current) +{ + double diff = past - current; + if (past * current < 0 && fabs(diff) > M_PI) + { // Crossed +PI/-PI[rad] border + diff = M_PI - fabs(past) + (M_PI - fabs(current)); // - to + + if (past > 0 && current < 0) + { // Case of + to - + diff = -diff; + } + } + return diff; +} \ No newline at end of file diff --git a/src/utils/rotation_tools.h b/src/utils/rotation_tools.h new file mode 100644 index 0000000..de83eb6 --- /dev/null +++ b/src/utils/rotation_tools.h @@ -0,0 +1,31 @@ +/* +MIT License + +Copyright (c) 2019 WHILL inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + + +/* Author: Hikaru Sugiura */ + +#pragma once + +double rad_diff(double past, double current); // Compares 2 radian values and returns difference between them. + From 5b24bbba513711e37bc915dfa44b762bf08c0bf2 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 18:16:28 +0900 Subject: [PATCH 12/41] Add Odometry clear Service --- src/ros_whill.cpp | 35 +++++++++++++++++++++++------------ 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 3c17421..2637139 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -33,6 +33,8 @@ SOFTWARE. #include "sensor_msgs/BatteryState.h" #include "nav_msgs/Odometry.h" +#include "std_srvs/Empty.h" + #include "tf/transform_broadcaster.h" #include "whill/WHILL.h" @@ -64,7 +66,7 @@ ros::Publisher ros_battery_state_publisher; ros::Publisher ros_odom_publisher; // TF Broadcaster -tf::TransformBroadcaster *odom_broadcaster; +tf::TransformBroadcaster *odom_broadcaster = nullptr; // // ROS Callbacks @@ -99,6 +101,13 @@ void ros_cmd_vel_callback(const geometry_msgs::Twist::ConstPtr &cmd_vel) } } +bool ros_srv_odom_clear_callback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) +{ + ROS_INFO("Clear Odometry"); + odom.reset(); + return true; +} + // // UART Interface // @@ -122,18 +131,17 @@ int serialWrite(std::vector &data) return 0; } + + // // WHILL // void whill_callback_data1(WHILL *caller) { - ros::Time currentTime = ros::Time::now(); - + // This function is called when receive Joy/Accelerometer/Gyro,etc. - ROS_INFO("Updated"); - ROS_INFO("%d",caller->joy.x); - ROS_INFO("%d", caller->joy.y); - ROS_INFO("interval:%d", caller->_interval); + + ros::Time currentTime = ros::Time::now(); // Joy sensor_msgs::Joy joy; @@ -143,6 +151,7 @@ void whill_callback_data1(WHILL *caller) joy.axes[1] = caller->joy.y / 100.0f; //Y ros_joystick_state_publisher.publish(joy); + // IMU sensor_msgs::Imu imu; imu.header.frame_id = "imu"; @@ -158,6 +167,7 @@ void whill_callback_data1(WHILL *caller) imu.linear_acceleration.z = caller->accelerometer.z * 9.80665; // G to m/ss ros_imu_publisher.publish(imu); + // Battery sensor_msgs::BatteryState batteryState; batteryState.voltage = 25.2; //[V] Spec voltage, since raw voltage is not provided. @@ -172,6 +182,7 @@ void whill_callback_data1(WHILL *caller) batteryState.location = "0"; ros_battery_state_publisher.publish(batteryState); + // JointState sensor_msgs::JointState jointState; jointState.name.resize(2); @@ -218,6 +229,7 @@ void whill_callback_data1(WHILL *caller) odom_msg.child_frame_id = "base_link"; ros_odom_publisher.publish(odom_msg); + // Odometory TF if (publish_tf) { @@ -250,12 +262,12 @@ int main(int argc, char **argv) std::string serialport; nh.param("serialport", serialport, "/dev/ttyUSB0"); - bool activate_experimental; - nh.param("activate_experimental", activate_experimental, false); + bool activate_experimental_topics; + nh.param("activate_experimental_topics", activate_experimental_topics, false); // Services //set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); - //clear_odom_service = nh.advertiseService("odom/clear", &clearOdom); + ros::ServiceServer clear_odom_service = nh.advertiseService("odom/clear", &ros_srv_odom_clear_callback); // Subscriber ros::Subscriber joystick_subscriber = nh.subscribe("controller/joy", 100, ros_joystick_callback); @@ -271,13 +283,12 @@ int main(int argc, char **argv) // TF Broadcaster odom_broadcaster = new tf::TransformBroadcaster; - if (activate_experimental) + if (activate_experimental_topics) { ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); } - // Node Param nh.getParam("send_interval", interval); if (interval < 10) From 84ce7a4b66f115af87109c934890fb90d7240d74 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 19:15:16 +0900 Subject: [PATCH 13/41] Clean variable names, structure --- src/ros_whill.cpp | 41 +++++++++++++++++++++++++---------------- 1 file changed, 25 insertions(+), 16 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 2637139..0f801a7 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -259,37 +259,31 @@ int main(int argc, char **argv) ros::init(argc, argv, "whill"); ros::NodeHandle nh("~"); - std::string serialport; - nh.param("serialport", serialport, "/dev/ttyUSB0"); - - bool activate_experimental_topics; - nh.param("activate_experimental_topics", activate_experimental_topics, false); // Services //set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); ros::ServiceServer clear_odom_service = nh.advertiseService("odom/clear", &ros_srv_odom_clear_callback); + // Subscriber ros::Subscriber joystick_subscriber = nh.subscribe("controller/joy", 100, ros_joystick_callback); ros::Subscriber twist_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); - // // Publishers + + // Publishers ros_joystick_state_publisher = nh.advertise("states/joy", 100); ros_jointstate_publisher = nh.advertise("states/jointState", 100); ros_imu_publisher = nh.advertise("states/imu", 100); ros_battery_state_publisher = nh.advertise("states/batteryState", 100); ros_odom_publisher = nh.advertise("odom", 100); + // TF Broadcaster odom_broadcaster = new tf::TransformBroadcaster; - if (activate_experimental_topics) - { - ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); - } - - // Node Param + // Parameters + // WHILL Report Packet Interval nh.getParam("send_interval", interval); if (interval < 10) { @@ -299,13 +293,28 @@ int main(int argc, char **argv) } ROS_INFO("param: send_interval=%d", interval); - std::string port = "/dev/tty.whill"; - unsigned long baud = 38400; + // Serial Port Device Name + std::string serialport; + nh.param("serialport", serialport, "/dev/ttyUSB0"); + // Disable publishing odometry tf + nh.param("publish_tf", publish_tf, true); + + // Enable Experimantal Topics + bool activate_experimental_topics; + nh.param("activate_experimental_topics", activate_experimental_topics, false); + if (activate_experimental_topics) + { + ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); + } + + + + unsigned long baud = 38400; serial::Timeout timeout = serial::Timeout::simpleTimeout(0); - timeout.write_timeout_multiplier = 5; + timeout.write_timeout_multiplier = 5; // Wait 5ms every bytes - ser = new serial::Serial(port, baud, timeout); + ser = new serial::Serial(serialport, baud, timeout); ser->flush(); whill = new WHILL(serialRead, serialWrite); From 65e934d4b2698356e911b1ffe1511e41d3d0bd35 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 19:26:14 +0900 Subject: [PATCH 14/41] Fix modelc.launch --- launch/modelc.launch | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/launch/modelc.launch b/launch/modelc.launch index 5621a48..ec312fb 100644 --- a/launch/modelc.launch +++ b/launch/modelc.launch @@ -7,12 +7,8 @@ - - - - - - + + From 70828883f80b2413c2429a6aff9e5125bb90495f Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 20:01:26 +0900 Subject: [PATCH 15/41] Add timestamp to publishing messages --- src/ros_whill.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 0f801a7..4d7dc8e 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -154,6 +154,7 @@ void whill_callback_data1(WHILL *caller) // IMU sensor_msgs::Imu imu; + imu.header.stamp = currentTime; imu.header.frame_id = "imu"; imu.orientation_covariance[0] = -1; // Orientation is unknown @@ -170,7 +171,8 @@ void whill_callback_data1(WHILL *caller) // Battery sensor_msgs::BatteryState batteryState; - batteryState.voltage = 25.2; //[V] Spec voltage, since raw voltage is not provided. + batteryState.header.stamp = currentTime; + batteryState.voltage = 25.2; //[V] Spec voltage, since raw voltage is not provided. batteryState.current = -caller->battery.current / 1000.0f; // mA -> A batteryState.charge = std::numeric_limits::quiet_NaN(); batteryState.design_capacity = 10.04; //[Ah] @@ -185,6 +187,7 @@ void whill_callback_data1(WHILL *caller) // JointState sensor_msgs::JointState jointState; + jointState.header.stamp = currentTime; jointState.name.resize(2); jointState.position.resize(2); jointState.velocity.resize(2); From d5260cd3b3474fac0315c8585f3685cb9199cdee Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 20:09:15 +0900 Subject: [PATCH 16/41] Fix node name in model.c --- launch/modelc.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/modelc.launch b/launch/modelc.launch index c4e247f..8d66580 100644 --- a/launch/modelc.launch +++ b/launch/modelc.launch @@ -10,7 +10,7 @@ - + From ccd89d0f57437c24ad4a941443732cc19647813f Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 9 May 2019 21:40:56 +0900 Subject: [PATCH 17/41] Maintain .srv and .msg files --- CMakeLists.txt | 6 ++++-- msg/SpeedPack.msg | 3 +++ msg/msgWhillSpeedProfile.msg | 11 ----------- srv/SetSpeedProfile.srv | 7 +++++++ srv/srvSetSpeedProfile.srv | 12 ------------ 5 files changed, 14 insertions(+), 25 deletions(-) create mode 100644 msg/SpeedPack.msg delete mode 100644 msg/msgWhillSpeedProfile.msg create mode 100644 srv/SetSpeedProfile.srv delete mode 100644 srv/srvSetSpeedProfile.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index e279502..bd5b115 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -54,12 +54,13 @@ add_subdirectory(./src/whill) ## Generate messages in the 'msg' folder add_message_files( FILES - ) + SpeedPack.msg + ) ## Generate services in the 'srv' folder add_service_files( FILES - srvSetSpeedProfile.srv + SetSpeedProfile.srv ) ## Generate actions in the 'action' folder @@ -73,6 +74,7 @@ add_subdirectory(./src/whill) generate_messages( DEPENDENCIES std_msgs + ros_whill ) ################################################ diff --git a/msg/SpeedPack.msg b/msg/SpeedPack.msg new file mode 100644 index 0000000..3e67cc2 --- /dev/null +++ b/msg/SpeedPack.msg @@ -0,0 +1,3 @@ +float32 speed +float32 acc +float32 dec \ No newline at end of file diff --git a/msg/msgWhillSpeedProfile.msg b/msg/msgWhillSpeedProfile.msg deleted file mode 100644 index cc7ad53..0000000 --- a/msg/msgWhillSpeedProfile.msg +++ /dev/null @@ -1,11 +0,0 @@ -int64 s1 -int64 fm1 -int64 fa1 -int64 fd1 -int64 rm1 -int64 ra1 -int64 rd1 -int64 tm1 -int64 ta1 -int64 td1 - diff --git a/srv/SetSpeedProfile.srv b/srv/SetSpeedProfile.srv new file mode 100644 index 0000000..89db6a0 --- /dev/null +++ b/srv/SetSpeedProfile.srv @@ -0,0 +1,7 @@ +ros_whill/SpeedPack forward +ros_whill/SpeedPack back +ros_whill/SpeedPack turn + +--- +bool success +string status_message diff --git a/srv/srvSetSpeedProfile.srv b/srv/srvSetSpeedProfile.srv deleted file mode 100644 index 4a508cc..0000000 --- a/srv/srvSetSpeedProfile.srv +++ /dev/null @@ -1,12 +0,0 @@ -int64 s1 -int64 fm1 -int64 fa1 -int64 fd1 -int64 rm1 -int64 ra1 -int64 rd1 -int64 tm1 -int64 ta1 -int64 td1 ---- -int64 result From 236b74036f049ee1ac5a108df645e49c4b8327bf Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 20:53:27 +0900 Subject: [PATCH 18/41] Set SpeedProfile Service --- CMakeLists.txt | 2 +- msg/SpeedPack.msg | 6 +- src/ros_whill.cpp | 152 +++++++++++++++++++++++++++-------- src/utils/unit_convert.cpp | 39 +++++++++ src/utils/unit_convert.h | 33 ++++++++ src/whill/CMakeLists.txt | 1 + src/whill/SpeedProfile.cpp | 63 +++++++++++++++ src/whill/WHILL.h | 141 ++++++++++++++++++-------------- src/whill/whill_commands.cpp | 34 ++++---- srv/SetSpeedProfile.srv | 2 +- 10 files changed, 357 insertions(+), 116 deletions(-) create mode 100644 src/utils/unit_convert.cpp create mode 100644 src/utils/unit_convert.h create mode 100644 src/whill/SpeedProfile.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index bd5b115..8fa2a8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,7 +138,7 @@ 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/ros_whill_node.cpp) -add_executable(ros_whill src/ros_whill.cpp src/utils/rotation_tools.cpp src/odom.cpp) +add_executable(ros_whill src/ros_whill.cpp src/odom.cpp src/utils/rotation_tools.cpp src/utils/unit_convert.cpp) ## Rename C++ executable without prefix diff --git a/msg/SpeedPack.msg b/msg/SpeedPack.msg index 3e67cc2..9597f0a 100644 --- a/msg/SpeedPack.msg +++ b/msg/SpeedPack.msg @@ -1,3 +1,3 @@ -float32 speed -float32 acc -float32 dec \ No newline at end of file +float32 speed # m/s +float32 acc # m/ss +float32 dec # m/ss \ No newline at end of file diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 4d7dc8e..e128430 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -35,12 +35,16 @@ SOFTWARE. #include "std_srvs/Empty.h" +#include "ros_whill/SpeedPack.h" +#include "ros_whill/SetSpeedProfile.h" + #include "tf/transform_broadcaster.h" -#include "whill/WHILL.h" -#include "serial/serial.h" // wjwood/Serial (ros-melodic-serial) +#include "whill/WHILL.h" +#include "serial/serial.h" // wjwood/Serial (ros-melodic-serial) #include "utils/rotation_tools.h" +#include "utils/unit_convert.h" #include "odom.h" // #include "./includes/subscriber.hpp" @@ -49,10 +53,11 @@ SOFTWARE. WHILL *whill = nullptr; Odometry odom; - // Global Parameters -int interval = 0; // WHILL Data frequency -bool publish_tf = true; // Enable publishing Odometry TF +int interval = 0; // WHILL Data frequency +bool publish_tf = true; // Enable publishing Odometry TF + + // // ROS Objects @@ -68,7 +73,9 @@ ros::Publisher ros_odom_publisher; // TF Broadcaster tf::TransformBroadcaster *odom_broadcaster = nullptr; -// + + +// // ROS Callbacks // void ros_joystick_callback(const sensor_msgs::Joy::ConstPtr &joy) @@ -108,6 +115,88 @@ bool ros_srv_odom_clear_callback(std_srvs::Empty::Request &req, std_srvs::Empty: return true; } +bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whill::SetSpeedProfile::Response &res) +{ + WHILL::SpeedProfile profile; + + profile.forward.speed = convert_mps_to_whill_speed(req.forward.speed); + profile.forward.acc = convert_mpss_to_whill_acc(req.forward.acc); + profile.forward.dec = convert_mpss_to_whill_acc(req.forward.dec); + profile.backward.speed = convert_mps_to_whill_speed(req.backward.speed); + profile.backward.acc = convert_mpss_to_whill_acc(req.backward.acc); + profile.backward.dec = convert_mpss_to_whill_acc(req.backward.dec); + profile.turn.speed = convert_mps_to_whill_speed(req.turn.speed); + profile.turn.acc = convert_mpss_to_whill_acc(req.turn.acc); + profile.turn.dec = convert_mpss_to_whill_acc(req.turn.dec); + + ROS_INFO("Setting Spped Profile"); + ROS_INFO("Forward\tSpeed:%d,Acc:%d,Dec:%d", profile.forward.speed, profile.forward.acc, profile.forward.dec); + ROS_INFO("Bacward\tSpeed:%d,Acc:%d,Dec:%d", profile.backward.speed, profile.backward.acc, profile.backward.dec); + ROS_INFO("Turn\tSpeed:%d,Acc:%d,Dec:%d\n", profile.turn.speed, profile.turn.acc, profile.turn.dec); + + // Validate Value + bool is_valid = false; + auto error = profile.check(); + ROS_INFO("error:%d", error); + switch (error) + { + case WHILL::SpeedProfile::Error::InvalidForwardSpeed: + res.status_message = "Invalid Forward Speed"; + break; + case WHILL::SpeedProfile::Error::InvalidBackwardSpeed: + res.status_message = "Invalid Backward Speed"; + break; + case WHILL::SpeedProfile::Error::InvalidTurnSpeed: + res.status_message = "Invalid Turn Speed"; + break; + case WHILL::SpeedProfile::Error::InvalidForwardAcc: + res.status_message = "Invalid Forward Acc"; + break; + case WHILL::SpeedProfile::Error::InvalidBackwardAcc: + res.status_message = "Invalid Backward Acc"; + break; + case WHILL::SpeedProfile::Error::InvalidTurnAcc: + res.status_message = "Invalid Turn Acc"; + break; + case WHILL::SpeedProfile::Error::InvalidForwardDec: + res.status_message = "Invalid Forward Dec"; + break; + case WHILL::SpeedProfile::Error::InvalidBackwardDec: + res.status_message = "Invalid Backward Dec"; + break; + case WHILL::SpeedProfile::Error::InvalidTurnDec: + res.status_message = "Invalid Turn Dec"; + break; + default: + is_valid = true; + } + + if(!is_valid){ + res.success = false; + return true; + } + + if (whill) + { + if (whill->setSpeedProfile(profile, 4)) + { + res.success = true; + res.status_message = "Set Speed Profile command has been sent."; + } + else + { + res.success = false; + res.status_message = "Invalid Value."; + } + } + else + { + res.success = false; + res.status_message = "whill instance is not initialzied."; + } + return true; +} + // // UART Interface // @@ -131,8 +220,6 @@ int serialWrite(std::vector &data) return 0; } - - // // WHILL // @@ -148,10 +235,9 @@ void whill_callback_data1(WHILL *caller) joy.header.stamp = currentTime; joy.axes.resize(2); joy.axes[0] = -caller->joy.x / 100.0f; //X - joy.axes[1] = caller->joy.y / 100.0f; //Y + joy.axes[1] = caller->joy.y / 100.0f; //Y ros_joystick_state_publisher.publish(joy); - // IMU sensor_msgs::Imu imu; imu.header.stamp = currentTime; @@ -168,14 +254,13 @@ void whill_callback_data1(WHILL *caller) imu.linear_acceleration.z = caller->accelerometer.z * 9.80665; // G to m/ss ros_imu_publisher.publish(imu); - // Battery sensor_msgs::BatteryState batteryState; batteryState.header.stamp = currentTime; batteryState.voltage = 25.2; //[V] Spec voltage, since raw voltage is not provided. batteryState.current = -caller->battery.current / 1000.0f; // mA -> A batteryState.charge = std::numeric_limits::quiet_NaN(); - batteryState.design_capacity = 10.04; //[Ah] + batteryState.design_capacity = 10.04; //[Ah] batteryState.percentage = caller->battery.level / 100.0f; // Percentage batteryState.power_supply_status = sensor_msgs::BatteryState::POWER_SUPPLY_STATUS_UNKNOWN; batteryState.power_supply_health = sensor_msgs::BatteryState::POWER_SUPPLY_HEALTH_UNKNOWN; @@ -184,7 +269,6 @@ void whill_callback_data1(WHILL *caller) batteryState.location = "0"; ros_battery_state_publisher.publish(batteryState); - // JointState sensor_msgs::JointState jointState; jointState.header.stamp = currentTime; @@ -198,30 +282,37 @@ void whill_callback_data1(WHILL *caller) jointState.position[1] = caller->right_motor.angle; //Rad static double joint_past[2] = {0.0f, 0.0f}; - if(caller->_interval == -1){ + if (caller->_interval == -1) + { // Standard, Constant specified time intervel jointState.velocity[0] = rad_diff(joint_past[0], jointState.position[0]) / (double(interval) / 1000.0f); // Rad/sec jointState.velocity[1] = rad_diff(joint_past[1], jointState.position[1]) / (double(interval) / 1000.0f); // Rad/sec - }else if(caller->_interval == 0){ + } + else if (caller->_interval == 0) + { // Experimental, Motor Control Disabled (= Brake Locked) jointState.velocity[0] = 0.0f; jointState.velocity[1] = 0.0f; - }else{ + } + else + { // Experimental, Under motor controlling - jointState.velocity[0] = rad_diff(joint_past[0], jointState.position[0]) / (double(caller->_interval) / 1000.0f); // Rad/sec - jointState.velocity[1] = rad_diff(joint_past[1], jointState.position[1]) / (double(caller->_interval) / 1000.0f); // Rad/sec + jointState.velocity[0] = rad_diff(joint_past[0], jointState.position[0]) / (double(caller->_interval) / 1000.0f); // Rad/sec + jointState.velocity[1] = rad_diff(joint_past[1], jointState.position[1]) / (double(caller->_interval) / 1000.0f); // Rad/sec } joint_past[0] = jointState.position[0]; joint_past[1] = jointState.position[1]; ros_jointstate_publisher.publish(jointState); - // Odometory - if(caller->_interval == -1){ + if (caller->_interval == -1) + { // Standard odom.update(jointState, interval / 1000.0f); - }else if(caller->_interval >= 0){ + } + else if (caller->_interval >= 0) + { // Experimental odom.update(jointState, caller->_interval / 1000.0f); } @@ -232,7 +323,6 @@ void whill_callback_data1(WHILL *caller) odom_msg.child_frame_id = "base_link"; ros_odom_publisher.publish(odom_msg); - // Odometory TF if (publish_tf) { @@ -240,7 +330,8 @@ void whill_callback_data1(WHILL *caller) odom_trans.header.stamp = currentTime; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link"; - if (odom_broadcaster){ + if (odom_broadcaster) + { odom_broadcaster->sendTransform(odom_trans); } } @@ -252,7 +343,6 @@ void whill_callback_powered_on(WHILL *caller) ROS_INFO("power_on"); } - // // Main // @@ -262,17 +352,15 @@ int main(int argc, char **argv) ros::init(argc, argv, "whill"); ros::NodeHandle nh("~"); - // Services //set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); - ros::ServiceServer clear_odom_service = nh.advertiseService("odom/clear", &ros_srv_odom_clear_callback); - + ros::ServiceServer clear_odom_service = nh.advertiseService("odom/clear", &ros_srv_odom_clear_callback); + ros::ServiceServer set_speed_profile_service = nh.advertiseService("speedProfile/set", &ros_srv_set_speed_profile); // Subscriber ros::Subscriber joystick_subscriber = nh.subscribe("controller/joy", 100, ros_joystick_callback); ros::Subscriber twist_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); - // Publishers ros_joystick_state_publisher = nh.advertise("states/joy", 100); ros_jointstate_publisher = nh.advertise("states/jointState", 100); @@ -280,11 +368,9 @@ int main(int argc, char **argv) ros_battery_state_publisher = nh.advertise("states/batteryState", 100); ros_odom_publisher = nh.advertise("odom", 100); - // TF Broadcaster odom_broadcaster = new tf::TransformBroadcaster; - // Parameters // WHILL Report Packet Interval nh.getParam("send_interval", interval); @@ -311,11 +397,9 @@ int main(int argc, char **argv) ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); } - - unsigned long baud = 38400; serial::Timeout timeout = serial::Timeout::simpleTimeout(0); - timeout.write_timeout_multiplier = 5; // Wait 5ms every bytes + timeout.write_timeout_multiplier = 5; // Wait 5ms every bytes ser = new serial::Serial(serialport, baud, timeout); ser->flush(); @@ -325,8 +409,6 @@ int main(int argc, char **argv) whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); whill->begin(20); // ms - - ros::AsyncSpinner spinner(1); spinner.start(); ros::Rate rate(100); diff --git a/src/utils/unit_convert.cpp b/src/utils/unit_convert.cpp new file mode 100644 index 0000000..fb8735e --- /dev/null +++ b/src/utils/unit_convert.cpp @@ -0,0 +1,39 @@ +/* +MIT License + +Copyright (c) 2019 WHILL inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* Author: Hikaru Sugiura */ + +#include + +uint8_t convert_mps_to_whill_speed(float mps) +{ // m/s to whill speed + float km_p_h = mps * 3.6; // m/s to km/h + return km_p_h * 10; // WHILL 60 means 6.0 km/h +} + +uint8_t convert_mpss_to_whill_acc(float mpss) +{ // m/ss to whill speed + double kmh_p_s = mpss * 3.6; // m/ss to km/h/s + return kmh_p_s /(0.4 * (32.0f / 256.0f)); +} \ No newline at end of file diff --git a/src/utils/unit_convert.h b/src/utils/unit_convert.h new file mode 100644 index 0000000..bb94e2d --- /dev/null +++ b/src/utils/unit_convert.h @@ -0,0 +1,33 @@ +/* +MIT License + +Copyright (c) 2019 WHILL inc. + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +*/ + +/* Author: Hikaru Sugiura */ + +#pragma once + +#include + +uint8_t convert_mps_to_whill_speed(float); + +uint8_t convert_mpss_to_whill_acc(float); \ No newline at end of file diff --git a/src/whill/CMakeLists.txt b/src/whill/CMakeLists.txt index f6f24eb..40e6a4b 100644 --- a/src/whill/CMakeLists.txt +++ b/src/whill/CMakeLists.txt @@ -3,6 +3,7 @@ add_library(lib_whill STATIC Packet.cpp PacketParser.cpp PacketReceiver.cpp + SpeedProfile.cpp whill_commands.cpp WHILL.cpp ) \ No newline at end of file diff --git a/src/whill/SpeedProfile.cpp b/src/whill/SpeedProfile.cpp new file mode 100644 index 0000000..d811ac0 --- /dev/null +++ b/src/whill/SpeedProfile.cpp @@ -0,0 +1,63 @@ +/* +The MIT License (MIT) + +Copyright (c) 2019 WHILL, + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +*/ + +/* Author : Hikaru Sugiura */ + + +#include "WHILL.h" +template +bool WHILL::SpeedProfile::checkRange(U min, T value, Z max) +{ + if(value < min)return false; + if(value > max)return false; + + return true; +} + +WHILL::SpeedProfile::Error WHILL::SpeedProfile::check() +{ + + if(!checkRange(8,this->forward.speed,60)) + return InvalidForwardSpeed; + if(!checkRange(10, this->forward.acc, 90)) + return InvalidForwardAcc; + if (!checkRange(40, this->forward.dec, 160)) + return InvalidForwardDec; + + if (!checkRange(8, this->forward.speed, 30)) + return InvalidBackwardSpeed; + if (!checkRange(10, this->forward.acc, 50)) + return InvalidBackwardAcc; + if (!checkRange(40, this->forward.dec, 90)) + return InvalidBackwardDec; + + if (!checkRange(8, this->turn.speed, 35)) + return InvalidTurnSpeed; + if (!checkRange(10, this->turn.acc, 60)) + return InvalidTurnAcc; + if (!checkRange(40, this->turn.dec, 160)) + return InvalidTurnDec; + + return NoError; +} \ No newline at end of file diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index a30a114..c9c3478 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -111,7 +111,7 @@ class WHILL PacketParser parser; // Experimantal - uint8_t past_time_ms = 0; + uint8_t past_time_ms = 0; static uint8_t calc_time_diff(uint8_t past, uint8_t current); public: @@ -133,73 +133,90 @@ class WHILL void refresh(); - void keep_joy_delay(unsigned long ms); - void delay(unsigned long ms); - - typedef struct + class SpeedProfile { - unsigned char forward_spped; - unsigned char forward_acceleration; - unsigned char forward_deceleration; - - unsigned char reverse_speed; - unsigned char reverse_acceleration; - unsigned char reverse_deceleration; - - unsigned char turn_speed; - unsigned char turn_acceleration; - unsigned char turn_deceleration; - } SpeedProfile; + private: + template + bool checkRange(U min, T value, Z max); - typedef struct + public: + enum Error { - int x; - int y; - int z; - } Data3D; - - typedef struct + NoError, + InvalidForwardSpeed, + InvalidBackwardSpeed, + InvalidTurnSpeed, + InvalidForwardAcc, + InvalidBackwardAcc, + InvalidTurnAcc, + InvalidForwardDec, + InvalidBackwardDec, + InvalidTurnDec, + }; + + class Pack { - int x; - int y; - } Joy; + public: + uint8_t speed; + uint8_t acc; + uint8_t dec; + }; + + Error check(); + Pack forward; + Pack backward; + Pack turn; + }; - typedef struct - { - unsigned char level; - signed long current; - } Battery; + typedef struct + { + int x; + int y; + int z; + } Data3D; - typedef struct - { - float angle; - int speed; - } Motor; - - Data3D accelerometer = {0}; - Data3D gyro = {0}; - Joy virtual_joy = {0}; - Joy joy = {0}; - Battery battery = {0}; - Motor left_motor = {0}; - Motor right_motor = {0}; - bool power = false; - int speed_mode_indicator = -1; - - // Experimental - int32_t _interval = -1; // ms - - //WHILL commands - void startSendingData0(unsigned int interval_ms, unsigned char speed_mode); - void startSendingData1(unsigned int interval_ms); - void stopSendingData(); - void setJoystick(int x, int y); - void setPower(bool power); - void setBatteryVoltaegeOut(bool out); - void setSpeedProfile(SpeedProfile * profile, unsigned char speed_mode); - - // Experimental - void setSpeed(float linear, float angular); // Linear:[m/s], Angular:[rad/s] + typedef struct + { + int x; + int y; + } Joy; + + typedef struct + { + unsigned char level; + signed long current; + } Battery; + + typedef struct + { + float angle; + int speed; + } Motor; + + Data3D accelerometer = {0}; + Data3D gyro = {0}; + Joy virtual_joy = {0}; + Joy joy = {0}; + Battery battery = {0}; + Motor left_motor = {0}; + Motor right_motor = {0}; + bool power = false; + int speed_mode_indicator = -1; + + // Experimental + int32_t _interval = -1; // ms + + //WHILL commands + void startSendingData0(unsigned int interval_ms, unsigned char speed_mode); + void startSendingData1(unsigned int interval_ms); + void stopSendingData(); + void setJoystick(int x, int y); + void setPower(bool power); + void setBatteryVoltaegeOut(bool out); + bool setSpeedProfile(SpeedProfile &profile, unsigned char speed_mode); + + // Experimental + void setSpeed(float linear, float angular); // Linear:[m/s], Angular:[rad/s] }; #endif \ No newline at end of file diff --git a/src/whill/whill_commands.cpp b/src/whill/whill_commands.cpp index 56ad21b..88146f9 100644 --- a/src/whill/whill_commands.cpp +++ b/src/whill/whill_commands.cpp @@ -23,13 +23,14 @@ THE SOFTWARE. */ #include "WHILL.h" +#include void WHILL::startSendingData0(unsigned int interval_ms,unsigned char speed_mode){ unsigned char payload[] = {0x00, //Start Sending Data 0x00, //Data0 (Speed profiles) (unsigned char)(interval_ms<<8 & 0xFF), (unsigned char)(interval_ms<<0 & 0xFF), - speed_mode}; + speed_mode}; Packet packet(payload,sizeof(payload)); packet.build(); transferPacket(&packet); @@ -75,23 +76,28 @@ void WHILL::setJoystick(int x,int y){ } -void WHILL::setSpeedProfile(SpeedProfile* profile,unsigned char speed_mode){ - unsigned char payload[] = {0x04, - profile->forward_spped, - profile->forward_acceleration, - profile->forward_deceleration, - profile->reverse_speed, - profile->reverse_acceleration, - profile->reverse_deceleration, - profile->turn_speed, - profile->turn_acceleration, - profile->turn_deceleration}; +bool WHILL::setSpeedProfile(SpeedProfile &profile,unsigned char speed_mode){ + + if(profile.check() != SpeedProfile::Error::NoError) + return false; + + unsigned char payload[] = {0x04, + speed_mode, + profile.forward.speed, + profile.forward.acc, + profile.forward.dec, + profile.backward.speed, + profile.backward.acc, + profile.backward.dec, + profile.turn.speed, + profile.turn.acc, + profile.turn.dec}; Packet packet(payload,sizeof(payload)); packet.build(); - transferPacket(&packet); + transferPacket(&packet); + return true; } - void WHILL::setBatteryVoltaegeOut(bool enable){ unsigned char payload[] = {0x05, (unsigned char)(enable ? 0x01 : 0x00)}; diff --git a/srv/SetSpeedProfile.srv b/srv/SetSpeedProfile.srv index 89db6a0..1632117 100644 --- a/srv/SetSpeedProfile.srv +++ b/srv/SetSpeedProfile.srv @@ -1,5 +1,5 @@ ros_whill/SpeedPack forward -ros_whill/SpeedPack back +ros_whill/SpeedPack backward ros_whill/SpeedPack turn --- From 4d6d2fee281a12c2ad4d79f11f80125c0b641b3a Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 20:55:30 +0900 Subject: [PATCH 19/41] Add comment to SpeedPack.msg --- msg/SpeedPack.msg | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/msg/SpeedPack.msg b/msg/SpeedPack.msg index 9597f0a..340fc2f 100644 --- a/msg/SpeedPack.msg +++ b/msg/SpeedPack.msg @@ -1,3 +1,3 @@ -float32 speed # m/s -float32 acc # m/ss -float32 dec # m/ss \ No newline at end of file +float32 speed # m/s or rad/s +float32 acc # m/ss or rad/ss +float32 dec # m/ss or rad/ss \ No newline at end of file From f46d57669ded24be1ac0fac2b5ac8054f5475e3d Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 21:21:43 +0900 Subject: [PATCH 20/41] Independent wheel_radius and wheel_tread --- src/odom.cpp | 11 ++++++++--- src/odom.h | 5 +++-- src/ros_whill.cpp | 1 + src/whill/WHILL.h | 3 +++ 4 files changed, 15 insertions(+), 5 deletions(-) diff --git a/src/odom.cpp b/src/odom.cpp index 2ad6330..ec44bcb 100644 --- a/src/odom.cpp +++ b/src/odom.cpp @@ -62,6 +62,11 @@ long double Odometry::confineRadian(long double rad) return rad; } +void Odometry::setParameters(double _wheel_radius, double _wheel_tread){ + this->wheel_radius = _wheel_radius; + this->wheel_tread = _wheel_tread; +} + void Odometry::update(sensor_msgs::JointState jointState, double dt) { if (dt == 0) @@ -70,11 +75,11 @@ void Odometry::update(sensor_msgs::JointState jointState, double dt) double angle_vel_r = jointState.velocity[1]; double angle_vel_l = -jointState.velocity[0]; - long double vr = angle_vel_r * wheel_radius_; - long double vl = angle_vel_l * wheel_radius_; + long double vr = angle_vel_r * wheel_radius; + long double vl = angle_vel_l * wheel_radius; long double delta_L = (vr + vl) / 2.0; - long double delta_theta = (vr - vl) / (2.0 * wheel_tread_); + long double delta_theta = (vr - vl) / (2.0 * wheel_tread); pose.x += delta_L * dt * cosl(pose.theta + delta_theta * dt / 2.0); pose.y += delta_L * dt * sinl(pose.theta + delta_theta * dt / 2.0); diff --git a/src/odom.h b/src/odom.h index b27522d..d67f5ec 100644 --- a/src/odom.h +++ b/src/odom.h @@ -36,14 +36,15 @@ class Odometry long double theta; } Space2D; - static constexpr double wheel_radius_ = 0.1325; - static constexpr double wheel_tread_ = 0.248; + double wheel_radius; + double wheel_tread; Space2D pose; Space2D velocity; public: Odometry(); + void setParameters(double _wheel_radius, double _wheel_tread); void update(sensor_msgs::JointState joint, double dt); void set(Space2D pose); void reset(); diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index e128430..ec2cac9 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -405,6 +405,7 @@ int main(int argc, char **argv) ser->flush(); whill = new WHILL(serialRead, serialWrite); + odom.setParameters(whill->wheel_radius, whill->wheel_tread); whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); whill->begin(20); // ms diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index c9c3478..47c0ed7 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -118,6 +118,9 @@ class WHILL WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)); void begin(unsigned int interval); + const float wheel_radius = 0.1325; + const float wheel_tread = 0.248; + //Callback enum EVENT { From 4a1c174129e2395662af3ef9a0395ac7784b70d3 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 21:22:27 +0900 Subject: [PATCH 21/41] Add EOL return at end --- src/utils/unit_convert.cpp | 2 +- src/utils/unit_convert.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/utils/unit_convert.cpp b/src/utils/unit_convert.cpp index fb8735e..142f694 100644 --- a/src/utils/unit_convert.cpp +++ b/src/utils/unit_convert.cpp @@ -36,4 +36,4 @@ uint8_t convert_mpss_to_whill_acc(float mpss) { // m/ss to whill speed double kmh_p_s = mpss * 3.6; // m/ss to km/h/s return kmh_p_s /(0.4 * (32.0f / 256.0f)); -} \ No newline at end of file +} diff --git a/src/utils/unit_convert.h b/src/utils/unit_convert.h index bb94e2d..be77a25 100644 --- a/src/utils/unit_convert.h +++ b/src/utils/unit_convert.h @@ -30,4 +30,4 @@ SOFTWARE. uint8_t convert_mps_to_whill_speed(float); -uint8_t convert_mpss_to_whill_acc(float); \ No newline at end of file +uint8_t convert_mpss_to_whill_acc(float); From 6877bd3d00d9b5d566d877ba2d8cc652572e070f Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 22:16:32 +0900 Subject: [PATCH 22/41] Change SpeedProfile Turning units to rad/s and rad/ss --- src/odom.cpp | 2 +- src/ros_whill.cpp | 33 +++++++++++++++++---------------- src/utils/unit_convert.cpp | 9 +++++++++ src/utils/unit_convert.h | 4 +++- src/whill/WHILL.h | 2 +- 5 files changed, 31 insertions(+), 19 deletions(-) diff --git a/src/odom.cpp b/src/odom.cpp index ec44bcb..94ea1e6 100644 --- a/src/odom.cpp +++ b/src/odom.cpp @@ -79,7 +79,7 @@ void Odometry::update(sensor_msgs::JointState jointState, double dt) long double vl = angle_vel_l * wheel_radius; long double delta_L = (vr + vl) / 2.0; - long double delta_theta = (vr - vl) / (2.0 * wheel_tread); + long double delta_theta = (vr - vl) / (wheel_tread); pose.x += delta_L * dt * cosl(pose.theta + delta_theta * dt / 2.0); pose.y += delta_L * dt * sinl(pose.theta + delta_theta * dt / 2.0); diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index ec2cac9..61f02f1 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -117,6 +117,13 @@ bool ros_srv_odom_clear_callback(std_srvs::Empty::Request &req, std_srvs::Empty: bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whill::SetSpeedProfile::Response &res) { + + if(whill == nullptr){ + res.success = false; + res.status_message = "whill instance is not initialzied."; + return true; + } + WHILL::SpeedProfile profile; profile.forward.speed = convert_mps_to_whill_speed(req.forward.speed); @@ -125,9 +132,9 @@ bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whi profile.backward.speed = convert_mps_to_whill_speed(req.backward.speed); profile.backward.acc = convert_mpss_to_whill_acc(req.backward.acc); profile.backward.dec = convert_mpss_to_whill_acc(req.backward.dec); - profile.turn.speed = convert_mps_to_whill_speed(req.turn.speed); - profile.turn.acc = convert_mpss_to_whill_acc(req.turn.acc); - profile.turn.dec = convert_mpss_to_whill_acc(req.turn.dec); + profile.turn.speed = convert_radps_to_whill_speed(whill->tread, req.turn.speed); + profile.turn.acc = convert_radpss_to_whill_acc(whill->tread,req.turn.acc); + profile.turn.dec = convert_radpss_to_whill_acc(whill->tread, req.turn.dec); ROS_INFO("Setting Spped Profile"); ROS_INFO("Forward\tSpeed:%d,Acc:%d,Dec:%d", profile.forward.speed, profile.forward.acc, profile.forward.dec); @@ -176,24 +183,18 @@ bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whi return true; } - if (whill) + + if (whill->setSpeedProfile(profile, 4)) { - if (whill->setSpeedProfile(profile, 4)) - { - res.success = true; - res.status_message = "Set Speed Profile command has been sent."; - } - else - { - res.success = false; - res.status_message = "Invalid Value."; - } + res.success = true; + res.status_message = "Set Speed Profile command has been sent."; } else { res.success = false; - res.status_message = "whill instance is not initialzied."; + res.status_message = "Invalid Value."; } + return true; } @@ -405,7 +406,7 @@ int main(int argc, char **argv) ser->flush(); whill = new WHILL(serialRead, serialWrite); - odom.setParameters(whill->wheel_radius, whill->wheel_tread); + odom.setParameters(whill->wheel_radius, whill->tread); whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); whill->begin(20); // ms diff --git a/src/utils/unit_convert.cpp b/src/utils/unit_convert.cpp index 142f694..64439e4 100644 --- a/src/utils/unit_convert.cpp +++ b/src/utils/unit_convert.cpp @@ -37,3 +37,12 @@ uint8_t convert_mpss_to_whill_acc(float mpss) double kmh_p_s = mpss * 3.6; // m/ss to km/h/s return kmh_p_s /(0.4 * (32.0f / 256.0f)); } + +uint8_t convert_radps_to_whill_speed(float tread, float radps){ + double mps = radps * tread / 2 * 2.0f; // Half Tread * Turning Fix, Wheel Speed will be Max/2 in spin turning. + return convert_mps_to_whill_speed(mps); +} +uint8_t convert_radpss_to_whill_acc(float tread, float radpss){ + double mpss = radpss * tread / 2 * 2.0f; // Half Tread * Turning Fix + return convert_mpss_to_whill_acc(mpss); +} \ No newline at end of file diff --git a/src/utils/unit_convert.h b/src/utils/unit_convert.h index be77a25..04dcb1c 100644 --- a/src/utils/unit_convert.h +++ b/src/utils/unit_convert.h @@ -29,5 +29,7 @@ SOFTWARE. #include uint8_t convert_mps_to_whill_speed(float); - uint8_t convert_mpss_to_whill_acc(float); + +uint8_t convert_radps_to_whill_speed(float tread,float radps); +uint8_t convert_radpss_to_whill_acc(float tread, float radpss); \ No newline at end of file diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index 47c0ed7..9be2210 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -119,7 +119,7 @@ class WHILL void begin(unsigned int interval); const float wheel_radius = 0.1325; - const float wheel_tread = 0.248; + const float tread = 0.496; //Callback enum EVENT From 472ce22c334f5c38d9e7abdc03831fe98e089984 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 22:51:08 +0900 Subject: [PATCH 23/41] Set intial Speed Profile when boot --- params/initial_speedprofile.yaml | 9 ++++++++ src/ros_whill.cpp | 35 ++++++++++++++++++++++++-------- 2 files changed, 35 insertions(+), 9 deletions(-) create mode 100644 params/initial_speedprofile.yaml diff --git a/params/initial_speedprofile.yaml b/params/initial_speedprofile.yaml new file mode 100644 index 0000000..5a1964e --- /dev/null +++ b/params/initial_speedprofile.yaml @@ -0,0 +1,9 @@ +init_speed/forward/speed: 0.567 +init_speed/forward/acc: 0.23 +init_speed/forward/dec: 1.25 +init_speed/backward/speed: 0.557 +init_speed/backward/acc: 0.34 +init_speed/backward/dec: 0.89 +init_speed/turn/speed: 0.8 +init_speed/turn/acc: 0.79 +init_speed/turn/dec: 1.8 diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 61f02f1..663df2b 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -57,8 +57,6 @@ Odometry odom; int interval = 0; // WHILL Data frequency bool publish_tf = true; // Enable publishing Odometry TF - - // // ROS Objects // @@ -73,8 +71,6 @@ ros::Publisher ros_odom_publisher; // TF Broadcaster tf::TransformBroadcaster *odom_broadcaster = nullptr; - - // // ROS Callbacks // @@ -118,7 +114,8 @@ bool ros_srv_odom_clear_callback(std_srvs::Empty::Request &req, std_srvs::Empty: bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whill::SetSpeedProfile::Response &res) { - if(whill == nullptr){ + if (whill == nullptr) + { res.success = false; res.status_message = "whill instance is not initialzied."; return true; @@ -133,7 +130,7 @@ bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whi profile.backward.acc = convert_mpss_to_whill_acc(req.backward.acc); profile.backward.dec = convert_mpss_to_whill_acc(req.backward.dec); profile.turn.speed = convert_radps_to_whill_speed(whill->tread, req.turn.speed); - profile.turn.acc = convert_radpss_to_whill_acc(whill->tread,req.turn.acc); + profile.turn.acc = convert_radpss_to_whill_acc(whill->tread, req.turn.acc); profile.turn.dec = convert_radpss_to_whill_acc(whill->tread, req.turn.dec); ROS_INFO("Setting Spped Profile"); @@ -144,7 +141,6 @@ bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whi // Validate Value bool is_valid = false; auto error = profile.check(); - ROS_INFO("error:%d", error); switch (error) { case WHILL::SpeedProfile::Error::InvalidForwardSpeed: @@ -178,12 +174,12 @@ bool ros_srv_set_speed_profile(ros_whill::SetSpeedProfile::Request &req, ros_whi is_valid = true; } - if(!is_valid){ + if (!is_valid) + { res.success = false; return true; } - if (whill->setSpeedProfile(profile, 4)) { res.success = true; @@ -409,6 +405,27 @@ int main(int argc, char **argv) odom.setParameters(whill->wheel_radius, whill->tread); whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); + + // Initial Speed Profile + ros_whill::SetSpeedProfile::Request init_speed_req; + if (nh.getParam("init_speed/forward/speed", init_speed_req.forward.speed) && + nh.getParam("init_speed/forward/acc", init_speed_req.forward.acc) && + nh.getParam("init_speed/forward/dec", init_speed_req.forward.dec) && + nh.getParam("init_speed/backward/speed", init_speed_req.backward.speed) && + nh.getParam("init_speed/backward/acc", init_speed_req.backward.acc) && + nh.getParam("init_speed/backward/dec", init_speed_req.backward.dec) && + nh.getParam("init_speed/turn/speed", init_speed_req.turn.speed) && + nh.getParam("init_speed/turn/acc", init_speed_req.turn.acc) && + nh.getParam("init_speed/turn/dec", init_speed_req.turn.dec) + ) + { + ros_whill::SetSpeedProfile::Response res; + ros_srv_set_speed_profile(init_speed_req,res); + if(res.success == false){ + ROS_INFO("Could not set Initial Profile."); + } + } + whill->begin(20); // ms ros::AsyncSpinner spinner(1); From a9a5cbc3e946f9675d11141865e1bbbe4aac0342 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 23:08:17 +0900 Subject: [PATCH 24/41] Load initial speed profile at launch file --- launch/modelc.launch | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/launch/modelc.launch b/launch/modelc.launch index 8d66580..862cfaa 100644 --- a/launch/modelc.launch +++ b/launch/modelc.launch @@ -11,7 +11,16 @@ + + + + + + + + + From bf55d715b810c2c4e0207d22784d7b387a4e1d7b Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 10 May 2019 23:09:14 +0900 Subject: [PATCH 25/41] Add comment, speed profile units --- params/initial_speedprofile.yaml | 21 ++++++++++++--------- srv/SetSpeedProfile.srv | 6 +++--- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/params/initial_speedprofile.yaml b/params/initial_speedprofile.yaml index 5a1964e..75fdfd9 100644 --- a/params/initial_speedprofile.yaml +++ b/params/initial_speedprofile.yaml @@ -1,9 +1,12 @@ -init_speed/forward/speed: 0.567 -init_speed/forward/acc: 0.23 -init_speed/forward/dec: 1.25 -init_speed/backward/speed: 0.557 -init_speed/backward/acc: 0.34 -init_speed/backward/dec: 0.89 -init_speed/turn/speed: 0.8 -init_speed/turn/acc: 0.79 -init_speed/turn/dec: 1.8 +# +# Initial Speed Profile +# +init_speed/forward/speed: 0.567 # [m/s] +init_speed/forward/acc: 0.23 # [m/ss] +init_speed/forward/dec: 1.25 # [m/ss] +init_speed/backward/speed: 0.557 # [m/s] +init_speed/backward/acc: 0.34 # [m/ss] +init_speed/backward/dec: 0.89 # [m/ss] +init_speed/turn/speed: 0.8 # [rad/s] +init_speed/turn/acc: 0.79 # [rad/ss] +init_speed/turn/dec: 1.8 # [rad/ss] diff --git a/srv/SetSpeedProfile.srv b/srv/SetSpeedProfile.srv index 1632117..e39b9ff 100644 --- a/srv/SetSpeedProfile.srv +++ b/srv/SetSpeedProfile.srv @@ -1,6 +1,6 @@ -ros_whill/SpeedPack forward -ros_whill/SpeedPack backward -ros_whill/SpeedPack turn +ros_whill/SpeedPack forward # m/s(Speed) and m/ss(Acceleration/Deceleration) +ros_whill/SpeedPack backward # m/s(Speed) and m/ss(Acceleration/Deceleration) +ros_whill/SpeedPack turn # rad/s(Speed) and rad/ss(Acceleration/Deceleration) --- bool success From 65aa54599ffb708edb6854c619e32dffa9e3e6b5 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 16 May 2019 17:57:19 +0900 Subject: [PATCH 26/41] Delete multiple same subscriber --- src/ros_whill.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 663df2b..3811f09 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -356,8 +356,7 @@ int main(int argc, char **argv) // Subscriber ros::Subscriber joystick_subscriber = nh.subscribe("controller/joy", 100, ros_joystick_callback); - ros::Subscriber twist_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); - + // Publishers ros_joystick_state_publisher = nh.advertise("states/joy", 100); ros_jointstate_publisher = nh.advertise("states/jointState", 100); @@ -387,10 +386,11 @@ int main(int argc, char **argv) nh.param("publish_tf", publish_tf, true); // Enable Experimantal Topics - bool activate_experimental_topics; - nh.param("activate_experimental_topics", activate_experimental_topics, false); - if (activate_experimental_topics) + bool experimental_topics; + nh.param("experimental_topics", experimental_topics, false); + if (experimental_topics == true) { + ROS_INFO("Experimental topics activated"); ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); } From 8900374e98034beb06615f2ad88e7187ef178cd5 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Thu, 16 May 2019 18:34:03 +0900 Subject: [PATCH 27/41] Fix controller/cmd_vel topic --- src/ros_whill.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 3811f09..7218bfb 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -388,10 +388,12 @@ int main(int argc, char **argv) // Enable Experimantal Topics bool experimental_topics; nh.param("experimental_topics", experimental_topics, false); + + ros::Subscriber cmd_vel_subscriber; if (experimental_topics == true) { ROS_INFO("Experimental topics activated"); - ros::Subscriber control_cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); + cmd_vel_subscriber = nh.subscribe("controller/cmd_vel", 100, ros_cmd_vel_callback); } unsigned long baud = 38400; From b2f83b734a93319cf98762ef0428e5d20b3018fa Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Mon, 20 May 2019 16:08:53 +0900 Subject: [PATCH 28/41] Reconnect process --- src/ros_whill.cpp | 118 +++++++++++++++++++++++++++++++++------------- 1 file changed, 84 insertions(+), 34 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 7218bfb..f1a48b7 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -23,6 +23,7 @@ SOFTWARE. */ #include +#include #include #include @@ -56,6 +57,7 @@ Odometry odom; // Global Parameters int interval = 0; // WHILL Data frequency bool publish_tf = true; // Enable publishing Odometry TF +ros::Time last_received; // // ROS Objects @@ -203,7 +205,11 @@ int serialRead(std::vector &data) { if (ser && ser->isOpen()) { - return ser->read(data, 30); // How many bytes read in one time. + int ret = ser->read(data, 30); + if(ret > 0){ + last_received = ros::Time::now(); + } + return ret; // How many bytes read in one time. } return 0; } @@ -400,44 +406,88 @@ int main(int argc, char **argv) serial::Timeout timeout = serial::Timeout::simpleTimeout(0); timeout.write_timeout_multiplier = 5; // Wait 5ms every bytes - ser = new serial::Serial(serialport, baud, timeout); - ser->flush(); - - whill = new WHILL(serialRead, serialWrite); - odom.setParameters(whill->wheel_radius, whill->tread); - whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); - whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); - - // Initial Speed Profile - ros_whill::SetSpeedProfile::Request init_speed_req; - if (nh.getParam("init_speed/forward/speed", init_speed_req.forward.speed) && - nh.getParam("init_speed/forward/acc", init_speed_req.forward.acc) && - nh.getParam("init_speed/forward/dec", init_speed_req.forward.dec) && - nh.getParam("init_speed/backward/speed", init_speed_req.backward.speed) && - nh.getParam("init_speed/backward/acc", init_speed_req.backward.acc) && - nh.getParam("init_speed/backward/dec", init_speed_req.backward.dec) && - nh.getParam("init_speed/turn/speed", init_speed_req.turn.speed) && - nh.getParam("init_speed/turn/acc", init_speed_req.turn.acc) && - nh.getParam("init_speed/turn/dec", init_speed_req.turn.dec) - ) - { - ros_whill::SetSpeedProfile::Response res; - ros_srv_set_speed_profile(init_speed_req,res); - if(res.success == false){ - ROS_INFO("Could not set Initial Profile."); - } - } - - whill->begin(20); // ms - ros::AsyncSpinner spinner(1); spinner.start(); - ros::Rate rate(100); + + while (ros::ok()) { - whill->refresh(); - rate.sleep(); + last_received = ros::Time::now(); + std::cout << "\n Port Opening." << std::flush; + + while (ros::ok()) + { + try + { + ser = new serial::Serial(serialport, baud, timeout); + break; + } + catch (...) + { + delete ser; + ser = nullptr; + ros::Duration(1.0).sleep(); + std::cout << "." << std::flush; + } + } + if(!ros::ok())break; + + ROS_INFO("Opened."); + ser->flush(); + + whill = new WHILL(serialRead, serialWrite); + whill->stopSendingData(); + + whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); + whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); + + odom.reset(); + odom.setParameters(whill->wheel_radius, whill->tread); + + // Initial Speed Profile + ros_whill::SetSpeedProfile::Request init_speed_req; + if (nh.getParam("init_speed/forward/speed", init_speed_req.forward.speed) && + nh.getParam("init_speed/forward/acc", init_speed_req.forward.acc) && + nh.getParam("init_speed/forward/dec", init_speed_req.forward.dec) && + nh.getParam("init_speed/backward/speed", init_speed_req.backward.speed) && + nh.getParam("init_speed/backward/acc", init_speed_req.backward.acc) && + nh.getParam("init_speed/backward/dec", init_speed_req.backward.dec) && + nh.getParam("init_speed/turn/speed", init_speed_req.turn.speed) && + nh.getParam("init_speed/turn/acc", init_speed_req.turn.acc) && + nh.getParam("init_speed/turn/dec", init_speed_req.turn.dec) + ) + { + ros_whill::SetSpeedProfile::Response res; + ros_srv_set_speed_profile(init_speed_req,res); + if(res.success == false){ + ROS_INFO("Could not set Initial Profile."); + } + } + + + whill->begin(20); // ms + + ros::Rate rate(100); + + while (ros::ok()) + { + whill->refresh(); + rate.sleep(); + if (abs((last_received - ros::Time::now()).toSec()) > 2.0) + { + ROS_INFO("Disconnect due to no longer packets received."); + ROS_WARN("Check your serial connection to WHILL."); + break; + } + } + + whill->setPower(false); + ser->close(); + delete ser; + ser = nullptr; + delete whill; + whill = nullptr; } spinner.stop(); From 7e370ee3f33cec1211d7347b189a1affc8db97a7 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Tue, 21 May 2019 13:22:12 +0900 Subject: [PATCH 29/41] Fix odom for experimental firmware --- src/odom.cpp | 7 +++++++ src/odom.h | 1 + src/ros_whill.cpp | 19 +++++++++++-------- src/whill/PacketParser.cpp | 4 ++-- src/whill/WHILL.cpp | 3 ++- src/whill/WHILL.h | 4 ++-- 6 files changed, 25 insertions(+), 13 deletions(-) diff --git a/src/odom.cpp b/src/odom.cpp index 94ea1e6..e653f7b 100644 --- a/src/odom.cpp +++ b/src/odom.cpp @@ -94,6 +94,13 @@ void Odometry::update(sensor_msgs::JointState jointState, double dt) return; } +void Odometry::zeroVelocity(void){ + velocity.x = 0; + velocity.y = 0; + velocity.theta = 0; + return; +} + void Odometry::reset() { Space2D poseZero = {0, 0, 0}; diff --git a/src/odom.h b/src/odom.h index d67f5ec..25c7dc6 100644 --- a/src/odom.h +++ b/src/odom.h @@ -46,6 +46,7 @@ class Odometry Odometry(); void setParameters(double _wheel_radius, double _wheel_tread); void update(sensor_msgs::JointState joint, double dt); + void zeroVelocity(void); void set(Space2D pose); void reset(); diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index f1a48b7..6273ecf 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -206,9 +206,6 @@ int serialRead(std::vector &data) if (ser && ser->isOpen()) { int ret = ser->read(data, 30); - if(ret > 0){ - last_received = ros::Time::now(); - } return ret; // How many bytes read in one time. } return 0; @@ -317,7 +314,11 @@ void whill_callback_data1(WHILL *caller) else if (caller->_interval >= 0) { // Experimental - odom.update(jointState, caller->_interval / 1000.0f); + if(caller->_interval == 0){ + odom.zeroVelocity(); + }else{ + odom.update(jointState, caller->_interval / 1000.0f); + } } nav_msgs::Odometry odom_msg = odom.getROSOdometry(); @@ -338,6 +339,8 @@ void whill_callback_data1(WHILL *caller) odom_broadcaster->sendTransform(odom_trans); } } + + last_received = ros::Time::now(); } void whill_callback_powered_on(WHILL *caller) @@ -409,7 +412,7 @@ int main(int argc, char **argv) ros::AsyncSpinner spinner(1); spinner.start(); - + while (ros::ok()) { @@ -438,12 +441,13 @@ int main(int argc, char **argv) whill = new WHILL(serialRead, serialWrite); whill->stopSendingData(); + + odom.reset(); + odom.setParameters(whill->wheel_radius, whill->tread); whill->register_callback(whill_callback_data1, WHILL::EVENT::CALLBACK_DATA1); whill->register_callback(whill_callback_powered_on, WHILL::EVENT::CALLBACK_POWER_ON); - odom.reset(); - odom.setParameters(whill->wheel_radius, whill->tread); // Initial Speed Profile ros_whill::SetSpeedProfile::Request init_speed_req; @@ -482,7 +486,6 @@ int main(int argc, char **argv) } } - whill->setPower(false); ser->close(); delete ser; ser = nullptr; diff --git a/src/whill/PacketParser.cpp b/src/whill/PacketParser.cpp index 010554d..4e47f69 100644 --- a/src/whill/PacketParser.cpp +++ b/src/whill/PacketParser.cpp @@ -88,9 +88,9 @@ void WHILL::PacketParser::parseDataset1(Packet* packet){ // Experimental, Set interval comming from Motor Controller if (packet->rawLength() == WHILL::Packet::DATASET1_LEN_V02){ - whill->_interval = 1; uint8_t time_ms = packet->payload[29]; - whill->_interval = WHILL::calc_time_diff(whill->past_time_ms,time_ms); + if (whill->past_time_ms < 0)whill->_interval = 0; + else whill->_interval = WHILL::calc_time_diff(whill->past_time_ms,time_ms); whill->past_time_ms = time_ms; } else diff --git a/src/whill/WHILL.cpp b/src/whill/WHILL.cpp index 4ef0942..e8c425a 100644 --- a/src/whill/WHILL.cpp +++ b/src/whill/WHILL.cpp @@ -35,9 +35,10 @@ WHILL::WHILL(int (*read)(std::vector &data), int (*write)(std::vectorstartSendingData1(interval); + this->past_time_ms = -1; } void WHILL::transferPacket(Packet* packet){ diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index 9be2210..58ec4c5 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -111,12 +111,12 @@ class WHILL PacketParser parser; // Experimantal - uint8_t past_time_ms = 0; + int16_t past_time_ms = -1; // not received any data yet if negative static uint8_t calc_time_diff(uint8_t past, uint8_t current); public: WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)); - void begin(unsigned int interval); + void begin(uint8_t interval); const float wheel_radius = 0.1325; const float tread = 0.496; From 19c15d14a7699453ab283c287b622535e2950059 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Tue, 21 May 2019 16:36:09 +0900 Subject: [PATCH 30/41] Modify power_on command --- src/ros_whill.cpp | 12 ++++++++++-- src/whill/WHILL.cpp | 3 ++- src/whill/WHILL.h | 3 ++- src/whill/whill_commands.cpp | 5 +++++ 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 6273ecf..7ee0780 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -27,6 +27,8 @@ SOFTWARE. #include #include +#include "unistd.h" + #include "ros/ros.h" #include "sensor_msgs/Joy.h" #include "sensor_msgs/JointState.h" @@ -220,6 +222,11 @@ int serialWrite(std::vector &data) return 0; } +void sleep_ms(uint32_t ms){ + usleep(ms * 1000); + return; +} + // // WHILL // @@ -439,9 +446,10 @@ int main(int argc, char **argv) ROS_INFO("Opened."); ser->flush(); - whill = new WHILL(serialRead, serialWrite); + whill = new WHILL(serialRead, serialWrite, sleep_ms); + whill->setPower(true); whill->stopSendingData(); - + odom.reset(); odom.setParameters(whill->wheel_radius, whill->tread); diff --git a/src/whill/WHILL.cpp b/src/whill/WHILL.cpp index e8c425a..a85e881 100644 --- a/src/whill/WHILL.cpp +++ b/src/whill/WHILL.cpp @@ -26,10 +26,11 @@ THE SOFTWARE. #include -WHILL::WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)) +WHILL::WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data), void (*sleep)(uint32_t ms)) { this->read = read; // Register UART read interface pointer this->write = write; // Register UART write interface pointer + this->sleep_ms = sleep; // Resigster platform-depended sleep function pointer parser.setParent(this); receiver.register_callback(&parser,&PacketParser::parsePacket); diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index 58ec4c5..7c4f400 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -103,6 +103,7 @@ class WHILL // Custom transceiver int (*read)(std::vector &data); // Returns how many bytes read actually int (*write)(std::vector &data); // Returns how many bytes wrote actually + void (*sleep_ms)(uint32_t ms); // Sleep function (ms) void receivePacket(); void transferPacket(Packet *packet); @@ -115,7 +116,7 @@ class WHILL static uint8_t calc_time_diff(uint8_t past, uint8_t current); public: - WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data)); + WHILL(int (*read)(std::vector &data), int (*write)(std::vector &data), void (*sleep)(uint32_t ms)); void begin(uint8_t interval); const float wheel_radius = 0.1325; diff --git a/src/whill/whill_commands.cpp b/src/whill/whill_commands.cpp index 88146f9..cbce111 100644 --- a/src/whill/whill_commands.cpp +++ b/src/whill/whill_commands.cpp @@ -55,10 +55,15 @@ void WHILL::stopSendingData(){ } void WHILL::setPower(bool power){ + + std::vector nop = {0x55, 0x55, 0x55, 0x55, 0x55, 0x55}; + unsigned char payload[] = {0x02, (unsigned char)(power ? 0x01 : 0x00)}; Packet packet(payload,sizeof(payload)); packet.build(); + write(nop); + sleep_ms(5); transferPacket(&packet); } From 4a5c8a6bde47143103fc9bacc5b5b38f1d8706d7 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Tue, 21 May 2019 20:55:18 +0900 Subject: [PATCH 31/41] Fix Segmentation Fault, UART Buffer Over Run --- src/whill/Packet.cpp | 2 ++ src/whill/PacketReceiver.cpp | 13 +++++++++++-- 2 files changed, 13 insertions(+), 2 deletions(-) diff --git a/src/whill/Packet.cpp b/src/whill/Packet.cpp index 99c101b..6cd844a 100644 --- a/src/whill/Packet.cpp +++ b/src/whill/Packet.cpp @@ -51,6 +51,8 @@ bool WHILL::Packet::setRaw(unsigned char* raw,int whole_length){ protocol_sign = raw[0]; len = raw[1]; + if(len >= MAX_LENGTH) return false; // Is not valid + int prefix = 2; int i = 0; for(i=0;i Packet::MAX_LENGTH){ + // Prevent Buffer Over Run + index = 0; + recording = false; + return -1; + } buf[index] = data; @@ -75,7 +81,10 @@ void WHILL::PacketReceiver::register_callback(PacketParser* obj,int(PacketParser bool WHILL::PacketReceiver::call_callback(){ Packet packet; - packet.setRaw(buf,index+1); + + if(packet.setRaw(buf,index+1) == false){ // If packet was broken + return false; + } if(callback != NULL){ From 3e5103c6980144c093e79ad8fe0f211eebe636d9 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Tue, 21 May 2019 20:55:57 +0900 Subject: [PATCH 32/41] Change func pointer NULL to nullptr --- src/whill/WHILL.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h index 7c4f400..13903ab 100644 --- a/src/whill/WHILL.h +++ b/src/whill/WHILL.h @@ -85,11 +85,11 @@ class WHILL unsigned char index = 0; bool recording = false; - void (*callback)() = NULL; + void (*callback)() = nullptr; bool call_callback(); - PacketParser *obj = NULL; - int (PacketParser::*method)(WHILL::Packet *packet) = NULL; + PacketParser *obj = nullptr; + int (PacketParser::*method)(WHILL::Packet *packet) = nullptr; public: int push(unsigned char data); From 13684644664f182a35d99b0a224d97aeb01c9177 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Tue, 21 May 2019 20:56:25 +0900 Subject: [PATCH 33/41] Implement safe delete --- src/ros_whill.cpp | 17 ++++++++++++----- 1 file changed, 12 insertions(+), 5 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 7ee0780..3ec85ff 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -61,6 +61,15 @@ int interval = 0; // WHILL Data frequency bool publish_tf = true; // Enable publishing Odometry TF ros::Time last_received; +template +void safeDelete(T *&p) +{ + if (p != NULL) + { + delete (p); + (p) = NULL; + } +} // // ROS Objects // @@ -435,7 +444,7 @@ int main(int argc, char **argv) } catch (...) { - delete ser; + safeDelete(ser); ser = nullptr; ros::Duration(1.0).sleep(); std::cout << "." << std::flush; @@ -495,10 +504,8 @@ int main(int argc, char **argv) } ser->close(); - delete ser; - ser = nullptr; - delete whill; - whill = nullptr; + safeDelete(ser); + safeDelete(whill); } spinner.stop(); From 0054c6b9e37ca6dad445f4bb9b3c21e9e8e3aca8 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 22 May 2019 16:14:13 +0900 Subject: [PATCH 34/41] Add Power on/off service --- src/ros_whill.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 3ec85ff..458f245 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -36,6 +36,7 @@ SOFTWARE. #include "sensor_msgs/BatteryState.h" #include "nav_msgs/Odometry.h" +#include "std_srvs/SetBool.h" #include "std_srvs/Empty.h" #include "ros_whill/SpeedPack.h" @@ -117,6 +118,19 @@ void ros_cmd_vel_callback(const geometry_msgs::Twist::ConstPtr &cmd_vel) } } +bool ros_srv_set_power(std_srvs::SetBool::Request &req,std_srvs::SetBool::Response &res){ + if (whill == nullptr) + { + res.success = false; + res.message = "whill instance is not initialzied."; + return true; + } + + whill->setPower(false); + res.success = true; + return true; +} + bool ros_srv_odom_clear_callback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) { ROS_INFO("Clear Odometry"); @@ -378,10 +392,11 @@ int main(int argc, char **argv) //set_power_service_service = nh.advertiseService("power/on", set_power_service_callback); ros::ServiceServer clear_odom_service = nh.advertiseService("odom/clear", &ros_srv_odom_clear_callback); ros::ServiceServer set_speed_profile_service = nh.advertiseService("speedProfile/set", &ros_srv_set_speed_profile); + ros::ServiceServer set_power_service = nh.advertiseService("power", &ros_srv_set_power); // Subscriber ros::Subscriber joystick_subscriber = nh.subscribe("controller/joy", 100, ros_joystick_callback); - + // Publishers ros_joystick_state_publisher = nh.advertise("states/joy", 100); ros_jointstate_publisher = nh.advertise("states/jointState", 100); @@ -486,7 +501,6 @@ int main(int argc, char **argv) } } - whill->begin(20); // ms ros::Rate rate(100); @@ -502,7 +516,7 @@ int main(int argc, char **argv) break; } } - + whill->setPower(false); ser->close(); safeDelete(ser); safeDelete(whill); From b5976060a2acc9111c4256109db80850a92ed865 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 22 May 2019 16:14:50 +0900 Subject: [PATCH 35/41] Power command nop only on power-on --- src/whill/whill_commands.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/whill/whill_commands.cpp b/src/whill/whill_commands.cpp index cbce111..d4a0c50 100644 --- a/src/whill/whill_commands.cpp +++ b/src/whill/whill_commands.cpp @@ -62,8 +62,10 @@ void WHILL::setPower(bool power){ (unsigned char)(power ? 0x01 : 0x00)}; Packet packet(payload,sizeof(payload)); packet.build(); - write(nop); - sleep_ms(5); + if(power){ + write(nop); + sleep_ms(5); + } transferPacket(&packet); } From ee911a9d5b923d33ab348583a5032966f07b60a6 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 22 May 2019 16:20:51 +0900 Subject: [PATCH 36/41] Fix set power service send always turn-off --- src/ros_whill.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index 458f245..a725898 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -126,7 +126,7 @@ bool ros_srv_set_power(std_srvs::SetBool::Request &req,std_srvs::SetBool::Respon return true; } - whill->setPower(false); + whill->setPower(req.data); res.success = true; return true; } @@ -429,6 +429,9 @@ int main(int argc, char **argv) bool experimental_topics; nh.param("experimental_topics", experimental_topics, false); + bool keep_connected; + nh.param("keep_connected", keep_connected, false); + ros::Subscriber cmd_vel_subscriber; if (experimental_topics == true) { @@ -509,7 +512,7 @@ int main(int argc, char **argv) { whill->refresh(); rate.sleep(); - if (abs((last_received - ros::Time::now()).toSec()) > 2.0) + if (keep_connected && (abs((last_received - ros::Time::now()).toSec()) > 2.0)) { ROS_INFO("Disconnect due to no longer packets received."); ROS_WARN("Check your serial connection to WHILL."); From 503ce3afd3e9536c7a7a210c2d224caf42ceeacb Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Wed, 22 May 2019 16:34:24 +0900 Subject: [PATCH 37/41] Fix invalid power-off --- src/ros_whill.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp index a725898..387e27b 100644 --- a/src/ros_whill.cpp +++ b/src/ros_whill.cpp @@ -519,7 +519,7 @@ int main(int argc, char **argv) break; } } - whill->setPower(false); + ser->close(); safeDelete(ser); safeDelete(whill); From 09c4a80099ced69a73a5b062180aae1c018fe3b0 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 24 May 2019 20:27:36 +0900 Subject: [PATCH 38/41] Add keep_connected param to launch file --- launch/modelc.launch | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/launch/modelc.launch b/launch/modelc.launch index 862cfaa..8114648 100644 --- a/launch/modelc.launch +++ b/launch/modelc.launch @@ -15,6 +15,12 @@ + + + From bc8d13911512ad0a4e0298f426a946abb3faab2a Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 24 May 2019 20:28:08 +0900 Subject: [PATCH 39/41] Change launch file --- launch/{modelc.launch => ros_whill.launch} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename launch/{modelc.launch => ros_whill.launch} (100%) diff --git a/launch/modelc.launch b/launch/ros_whill.launch similarity index 100% rename from launch/modelc.launch rename to launch/ros_whill.launch From 7db6aa262d49f8e84c575da352de5a44072b8885 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 24 May 2019 20:28:26 +0900 Subject: [PATCH 40/41] Delete teleop launch --- launch/teleop_joy.launch | 8 -------- 1 file changed, 8 deletions(-) delete mode 100644 launch/teleop_joy.launch diff --git a/launch/teleop_joy.launch b/launch/teleop_joy.launch deleted file mode 100644 index 4c2bf8d..0000000 --- a/launch/teleop_joy.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file From ea5f8798e2c86f48d6f79e079203d7472221b415 Mon Sep 17 00:00:00 2001 From: Hikaru Sugiura Date: Fri, 24 May 2019 21:02:28 +0900 Subject: [PATCH 41/41] Edit README.md --- README.md | 88 +++++++++++++++++++++++++++++++++++-------------------- 1 file changed, 56 insertions(+), 32 deletions(-) diff --git a/README.md b/README.md index 0bb4342..cc05d14 100644 --- a/README.md +++ b/README.md @@ -5,48 +5,77 @@ For general questions and requests, please visit https://whill.zendesk.com/hc/ja +## Requirements +- ROS Melodic + ## ROS API ### Subscribed Topics -#### /whill/controller/joy [(sensor_msgs/Joy)](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) +#### ~controller/joy [(sensor_msgs/Joy)](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) - Virtual WHILL joystick input. You can controll WHILL via this topic. ### Published Topics -#### /whill/states/joy [(sensor_msgs/Joy)](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) +#### ~states/joy [(sensor_msgs/Joy)](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html) - Joystick status -#### /whill/states/jointState [(sensor_msgs/JointState)](http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html) +#### ~states/jointState [(sensor_msgs/JointState)](http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html) - Wheel rotate position(rad) and rotation velocity(rad/s) -#### /whill/states/imu [(sensor_msgs/Imu)](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) +#### ~states/imu [(sensor_msgs/Imu)](http://docs.ros.org/api/sensor_msgs/html/msg/Imu.html) - IMU measured data. -#### /whill/states/batteryState [(sensor_msgs/BatteryState)](http://docs.ros.org/api/sensor_msgs/html/msg/BatteryState.html) +#### ~states/batteryState [(sensor_msgs/BatteryState)](http://docs.ros.org/api/sensor_msgs/html/msg/BatteryState.html) - Battery information -## Requirements -- Ubuntu 16.04 -- ROS kinetic +### Services -## Build -In your shell: -```sh -cd ~/catkin_ws -catkin_make -rospack profile +#### ~odom/clear [std_srvs/Empty] +Clear Odometry + +#### ~power [std_srvs/SetBool] +True to send power on command, false to power off. + +#### ~speedProfile/set [ros_whill/SetSpeedProfile] +You can set WHILL speed profile for `~controller/joy` topic. ``` -ros_whill package is sometimes not recognized if you not set "rospack profile". (After executed "rosrun ros_whill", you might see the error message "not found package".) +ros_whill/SpeedPack forward + float32 speed # m/s + float32 acc # m/ss + float32 dec # m/ss +ros_whill/SpeedPack backward + float32 speed # m/s + float32 acc # m/ss + float32 dec # m/ss +ros_whill/SpeedPack turn + float32 speed # rad/s + float32 acc # rad/ss + float32 dec # rad/ss +--- +bool success +string status_message -### Build only ros_whill package -```sh -catkin_make -DCATKIN_WHITELIST_PACKAGES="ros_whill" ``` -## SerialPort Settings +### Parameters + +### ~init_speed/* +See: + +### ~keep_connected (Bool, default:false) +Set true to try to keep connected by re-opening port and sending power-on command. Though the WHILL automticarry wakes up even you turn off manualy or by power-off command. + +### ~publish_tf (Bool, defualt: true) +False to stop publishing `odom` to `base_link` tf. If other node publishs, set value to false. + +### ~serialport (String, default:/dev/ttyUSB0) + + +## SerialPort Setting for ros_whill.launch +The `ros_whill.launch` is using environmental variable `TTY_WHILL` for specify which serial port to be used. ### Set @@ -76,17 +105,6 @@ source ~/.zshrc echo $TTY_WHILL # -> Should be /dev/[YOUR SERIAL PORT DEVICE] ``` -## Launch -```sh -roslaunch ros_whill modelc.launch -``` - -### Set serial port as an argument of the launch file -```sh -roslaunch ros_whill modelc.launch serialport:=/dev/[YOUR SERIAL PORT DEVICE] -``` - - ### In the case of opening serial port failed Edit @@ -99,7 +117,13 @@ And add: KERNEL=="ttyUSB[0-9]*", MODE="0666" ``` -## Change Speed Profile + +## Launch with Model +```sh +$ roslaunch ros_whill ros_whill.launch +``` + +### Set serial port as an argument of the launch file ```sh -rosservice call /set_speed_profile {4, 15, 16, 64, 10, 16, 56, 10, 56, 72} +roslaunch ros_whill ros_whill.launch serialport:=/dev/[YOUR SERIAL PORT DEVICE] ```