├── msg ├── CanFrame.msg ├── BrakeReport.msg ├── EnableDisable.msg ├── FaultReport.msg ├── BrakeCommand.msg ├── SteeringReport.msg ├── ThrottleReport.msg ├── SteeringCommand.msg ├── ThrottleCommand.msg ├── FaultReportData.msg ├── CanFrameData.msg ├── FaultOriginId.msg ├── BrakeReportData.msg ├── SteeringReportData.msg └── ThrottleReportData.msg ├── example ├── apollo.launch ├── example.launch ├── roscco_apollo.h ├── roscco_apollo.cpp └── roscco_teleop.cpp ├── test ├── test_oscc_to_ros.launch ├── test_ros_to_oscc.launch ├── test_roscco_mem.launch ├── include │ └── oscc.h ├── oscc_fixture.c ├── test_ros_to_oscc.cpp └── test_oscc_to_ros.cpp ├── .gitmodules ├── include └── roscco │ ├── pid_control.h │ ├── ros_to_oscc.h │ └── oscc_to_ros.h ├── Dockerfile ├── src ├── roscco_node.cpp ├── pid_control.cpp ├── ros_to_oscc.cpp └── oscc_to_ros.cpp ├── LICENSE ├── package.xml ├── Jenkinsfile ├── README.md └── CMakeLists.txt /msg/CanFrame.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | CanFrameData frame 3 | -------------------------------------------------------------------------------- /msg/BrakeReport.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | BrakeReportData data 3 | -------------------------------------------------------------------------------- /msg/EnableDisable.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | bool enable_control 3 | -------------------------------------------------------------------------------- /msg/FaultReport.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | FaultReportData data 3 | -------------------------------------------------------------------------------- /msg/BrakeCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 brake_position 3 | -------------------------------------------------------------------------------- /msg/SteeringReport.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | SteeringReportData data 3 | -------------------------------------------------------------------------------- /msg/ThrottleReport.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | ThrottleReportData data 3 | -------------------------------------------------------------------------------- /msg/SteeringCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 steering_torque 3 | -------------------------------------------------------------------------------- /msg/ThrottleCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 throttle_position 3 | -------------------------------------------------------------------------------- /msg/FaultReportData.msg: -------------------------------------------------------------------------------- 1 | uint8[2] magic 2 | uint32 fault_origin_id 3 | uint8 dtcs 4 | uint8 reserved 5 | -------------------------------------------------------------------------------- /msg/CanFrameData.msg: -------------------------------------------------------------------------------- 1 | uint8 CAN_FRAME_DATA_MAX_SIZE=8 2 | uint32 can_id 3 | uint8 can_dlc 4 | uint8[8] data 5 | -------------------------------------------------------------------------------- /msg/FaultOriginId.msg: -------------------------------------------------------------------------------- 1 | uint32 FaultOriginBrake=0 2 | uint32 FaultOriginSteering=1 3 | uint32 FaultOriginThrottle=2 4 | -------------------------------------------------------------------------------- /msg/BrakeReportData.msg: -------------------------------------------------------------------------------- 1 | uint8[2] magic 2 | uint8 enabled 3 | uint8 operator_override 4 | uint8 dtcs 5 | uint8[3] reserved 6 | -------------------------------------------------------------------------------- /msg/SteeringReportData.msg: -------------------------------------------------------------------------------- 1 | uint8[2] magic 2 | uint8 enabled 3 | uint8 operator_override 4 | uint8 dtcs 5 | uint8[3] reserved 6 | -------------------------------------------------------------------------------- /msg/ThrottleReportData.msg: -------------------------------------------------------------------------------- 1 | uint8[2] magic 2 | uint8 enabled 3 | uint8 operator_override 4 | uint8 dtcs 5 | uint8[3] reserved 6 | -------------------------------------------------------------------------------- /example/apollo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /test/test_oscc_to_ros.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /test/test_ros_to_oscc.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /example/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /test/test_roscco_mem.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "oscc"] 2 | path = oscc 3 | url = https://github.com/PolySync/oscc.git 4 | [submodule "test/rapidcheck"] 5 | path = test/rapidcheck 6 | url = https://github.com/emil-e/rapidcheck.git 7 | [submodule "roscpp_code_format"] 8 | path = roscpp_code_format 9 | url = https://github.com/davetcoleman/roscpp_code_format.git 10 | -------------------------------------------------------------------------------- /include/roscco/pid_control.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | typedef struct 4 | { 5 | double max; 6 | double min; 7 | double p_term; 8 | double i_term; 9 | double d_term; 10 | double i_max; 11 | } pid_terms; 12 | 13 | typedef struct 14 | { 15 | double integral; 16 | double pre_error; 17 | double setpoint; 18 | } pid_state; 19 | 20 | /** 21 | * @brief define a PID state 22 | * 23 | * @param setpoint/target 24 | * @param PID state 25 | */ 26 | void createPIDState( double setpoint, pid_state *state ); 27 | 28 | /** 29 | * @brief PID controller 30 | * 31 | * @param PID terms (max, min, p_term, i_term, d_term, i_max) 32 | * @param PID state 33 | * @param measured process variable 34 | */ 35 | double pidController( pid_terms *terms, pid_state *state, double position ); 36 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:16.04 2 | 3 | WORKDIR /app 4 | 5 | # common packages 6 | RUN apt-get update && \ 7 | apt-get install -y \ 8 | build-essential cmake git wget && \ 9 | rm -rf /var/lib/apt/lists/* 10 | 11 | # add ROS packages to apt package manager 12 | RUN sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list' 13 | 14 | RUN apt-key adv --keyserver keyserver.ubuntu.com --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116 15 | 16 | # install ros 17 | RUN apt-get update && \ 18 | apt-get install -y ros-kinetic-ros-base 19 | 20 | # install arduino toolchain 21 | RUN wget -nv http://arduino.cc/download.php?f=/arduino-1.8.5-linux64.tar.xz -O arduino-1.8.5.tar.xz 22 | 23 | RUN tar -xf arduino-1.8.5.tar.xz && \ 24 | cd arduino-1.8.5 && \ 25 | mkdir -p /usr/share/arduino && \ 26 | cp -R * /usr/share/arduino 27 | -------------------------------------------------------------------------------- /src/roscco_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | extern "C" { 4 | #include 5 | } 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | int main(int argc, char* argv[]) 13 | { 14 | ros::init(argc, argv, "roscco_node"); 15 | 16 | ros::NodeHandle public_nh; 17 | ros::NodeHandle private_nh("~"); 18 | 19 | int can_channel; 20 | private_nh.param("can_channel", can_channel, 0); 21 | 22 | oscc_result_t ret = OSCC_ERROR; 23 | 24 | ret = oscc_init(); 25 | 26 | if (ret != OSCC_OK) 27 | { 28 | ROS_ERROR("Could not initialize OSCC"); 29 | } 30 | 31 | RosToOscc subcriber(&public_nh, &private_nh); 32 | OsccToRos publisher(&public_nh, &private_nh); 33 | 34 | ros::spin(); 35 | 36 | ret = oscc_disable(); 37 | 38 | if (ret != OSCC_OK) 39 | { 40 | ROS_ERROR("Could not disable OSCC"); 41 | } 42 | 43 | ret = oscc_close(can_channel); 44 | 45 | if (ret != OSCC_OK) 46 | { 47 | ROS_ERROR("Could not close OSCC connection"); 48 | } 49 | 50 | ros::waitForShutdown(); 51 | } 52 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | ===================== 3 | 4 | Copyright (c) 2017 PolySync Technologies, Inc. All Rights Reserved. 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /src/pid_control.cpp: -------------------------------------------------------------------------------- 1 | #include "roscco/pid_control.h" 2 | 3 | void createPIDState( double setpoint, pid_state *state ) 4 | { 5 | state->integral = 0; 6 | state->pre_error = 0; 7 | state->setpoint = setpoint; 8 | } 9 | 10 | double pidController( pid_terms *terms, pid_state *state, double position ) 11 | { 12 | ///Calculate error 13 | double error = position - state->setpoint; 14 | 15 | ///Proportional term 16 | double Pout = terms->p_term*error; 17 | 18 | ///Integral term 19 | state->integral += error; 20 | 21 | if( state->integral > terms->i_max ) 22 | { 23 | state->integral = terms->i_max; 24 | } 25 | if( state->integral < -terms->i_max ) 26 | { 27 | state->integral = -terms->i_max; 28 | } 29 | 30 | double Iout = terms->i_term*state->integral; 31 | 32 | ///Derivative term 33 | double derivative = error - state->pre_error; 34 | double Dout = terms->d_term*derivative; 35 | 36 | ///Calculate total output 37 | double output = Pout + Iout + Dout; 38 | 39 | ///Restrict to max/min 40 | if( output > terms->max ) 41 | { 42 | output = terms->max; 43 | } 44 | else 45 | { 46 | if( output < terms->min ) 47 | { 48 | output = terms->min; 49 | } 50 | } 51 | 52 | ///save error to previous error 53 | state->pre_error = error; 54 | 55 | return output; 56 | } 57 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | roscco 4 | 1.0.0 5 | An OSCC to ROS interface 6 | 7 | 8 | PolySync 9 | 10 | 11 | 12 | MIT 13 | 14 | 15 | 16 | 17 | https://github.com/PolySync/roscco/ 18 | 19 | 20 | 21 | 22 | Robert Brown 23 | 24 | 25 | 26 | 27 | catkin 28 | message_generation 29 | roscpp 30 | std_msgs 31 | rostest 32 | message_runtime 33 | roscpp 34 | std_msgs 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /test/include/oscc.h: -------------------------------------------------------------------------------- 1 | #ifndef _OSCC_H 2 | #define _OSCC_H 3 | 4 | #include 5 | 6 | #include "brake_can_protocol.h" 7 | #include "fault_can_protocol.h" 8 | #include "steering_can_protocol.h" 9 | #include "throttle_can_protocol.h" 10 | 11 | typedef enum { OSCC_OK, OSCC_ERROR, OSCC_WARNING } oscc_result_t; 12 | 13 | oscc_result_t oscc_publish_steering_torque(double input); 14 | 15 | oscc_result_t oscc_publish_brake_position(double input); 16 | 17 | oscc_result_t oscc_publish_throttle_position(double input); 18 | 19 | oscc_result_t oscc_enable(void); 20 | 21 | oscc_result_t oscc_disable(void); 22 | 23 | oscc_result_t oscc_subscribe_to_brake_reports(void (*callback)(oscc_brake_report_s* report)); 24 | 25 | oscc_result_t oscc_subscribe_to_throttle_reports(void (*callback)(oscc_throttle_report_s* report)); 26 | 27 | oscc_result_t oscc_subscribe_to_steering_reports(void (*callback)(oscc_steering_report_s* report)); 28 | 29 | oscc_result_t oscc_subscribe_to_fault_reports(void (*callback)(oscc_fault_report_s* report)); 30 | 31 | oscc_result_t oscc_subscribe_to_obd_messages(void (*callback)(struct can_frame* frame)); 32 | 33 | // Expose the Callback for testing API 34 | void (*steering_report_callback)(oscc_steering_report_s* report); 35 | 36 | void (*brake_report_callback)(oscc_brake_report_s* report); 37 | 38 | void (*throttle_report_callback)(oscc_throttle_report_s* report); 39 | 40 | void (*fault_report_callback)(oscc_fault_report_s* report); 41 | 42 | void (*obd_frame_callback)(struct can_frame* frame); 43 | 44 | // Expose variables that are set for validating API call 45 | double get_brake_command(); 46 | 47 | double get_steering_torque(); 48 | 49 | double get_throttle_command(); 50 | 51 | int get_enable_disable(); 52 | 53 | #endif /* _OSCC_H */ 54 | -------------------------------------------------------------------------------- /test/oscc_fixture.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static double steering_torque; 4 | static double brake_command; 5 | static double throttle_command; 6 | int enabled; 7 | 8 | double get_brake_command() { return brake_command; } 9 | 10 | double get_steering_torque() { return steering_torque; } 11 | 12 | double get_throttle_command() { return throttle_command; } 13 | 14 | int get_enable_disable() { return enabled; } 15 | 16 | oscc_result_t oscc_publish_steering_torque(double input) 17 | { 18 | steering_torque = input; 19 | 20 | return OSCC_OK; 21 | } 22 | 23 | oscc_result_t oscc_publish_brake_position(double input) 24 | { 25 | brake_command = input; 26 | 27 | return OSCC_OK; 28 | } 29 | 30 | oscc_result_t oscc_publish_throttle_position(double input) 31 | { 32 | throttle_command = input; 33 | 34 | return OSCC_OK; 35 | } 36 | 37 | oscc_result_t oscc_enable(void) 38 | { 39 | ++enabled; 40 | 41 | return OSCC_OK; 42 | } 43 | 44 | oscc_result_t oscc_disable(void) 45 | { 46 | enabled = 0; 47 | 48 | return OSCC_OK; 49 | } 50 | 51 | oscc_result_t 52 | oscc_subscribe_to_brake_reports(void (*callback)(oscc_brake_report_s* report)) 53 | { 54 | brake_report_callback = callback; 55 | return OSCC_OK; 56 | } 57 | 58 | oscc_result_t oscc_subscribe_to_throttle_reports( 59 | void (*callback)(oscc_throttle_report_s* report)) 60 | { 61 | throttle_report_callback = callback; 62 | return OSCC_OK; 63 | } 64 | 65 | oscc_result_t oscc_subscribe_to_steering_reports( 66 | void (*callback)(oscc_steering_report_s* report)) 67 | { 68 | steering_report_callback = callback; 69 | return OSCC_OK; 70 | } 71 | 72 | oscc_result_t 73 | oscc_subscribe_to_fault_reports(void (*callback)(oscc_fault_report_s* report)) 74 | { 75 | fault_report_callback = callback; 76 | return OSCC_OK; 77 | } 78 | 79 | oscc_result_t 80 | oscc_subscribe_to_obd_messages(void (*callback)(struct can_frame* frame)) 81 | { 82 | obd_frame_callback = callback; 83 | return OSCC_OK; 84 | } 85 | -------------------------------------------------------------------------------- /Jenkinsfile: -------------------------------------------------------------------------------- 1 | #!groovy 2 | 3 | node() { 4 | def builds = [:] 5 | def platforms = [:] 6 | 7 | sh 'mkdir -p catkin_ws/src/roscco' 8 | dir('catkin_ws/src/roscco') { 9 | checkout scm 10 | sh "git submodule update --init --recursive" 11 | 12 | def image = docker.build("catkin_make-build:${env.BUILD_ID}") 13 | 14 | 15 | def output = image.inside { 16 | sh returnStdout: true, script: "cmake -LA ./oscc/firmware | grep 'VEHICLE_VALUES' | cut -d'=' -f 2" 17 | } 18 | 19 | platforms = output.trim().tokenize(';')\ 20 | } 21 | 22 | for(int j=0; j 5 | 6 | extern "C" { 7 | #include 8 | } 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | class RosToOscc 18 | { 19 | public: 20 | /** 21 | * @brief RosToOscc class initializer 22 | * 23 | * This function constructs ROS subscribers which can publish messages to OSCC API. 24 | * 25 | * @param public_nh The public node handle to use for ROS subscribers. 26 | * @param private_nh The private node handle for ROS parameters. 27 | */ 28 | RosToOscc(ros::NodeHandle* public_nh, ros::NodeHandle* private_nh); 29 | 30 | /** 31 | * @brief Callback function to publish ROS BrakeCommand messages to OSCC. 32 | * 33 | * This function is a callback that consume a ROS BrakeCommand message and publishes them to the OSCC API. 34 | * 35 | * @param msg ROS BrakeCommand message to be consumed. 36 | */ 37 | void brakeCommandCallback(const roscco::BrakeCommand::ConstPtr& msg); 38 | 39 | /** 40 | * @brief Callback function to publish ROS SteeringCommand messages to OSCC. 41 | * 42 | * This function is a callback that consumes a ROS SteeringCommand message and publishes them to the OSCC API. 43 | * 44 | * @param msg ROS SteeringCommand message to be consumed. 45 | */ 46 | void steeringCommandCallback(const roscco::SteeringCommand::ConstPtr& msg); 47 | 48 | /** 49 | * @brief Callback function to publish ROS ThrottleCommand messages to OSCC. 50 | * 51 | * This function is a callback that consumes a ROS ThrottleCommand message and publishes them to the OSCC API. 52 | * 53 | * @param msg ROS ThrottleCommand message to be consumed. 54 | */ 55 | void throttleCommandCallback(const roscco::ThrottleCommand::ConstPtr& msg); 56 | 57 | /** 58 | * @brief Callback function to publish ROS EnableDisable messages to OSCC. 59 | * 60 | * This function is a callback that consumes a ROS EnableDisable message and publishes them to the OSCC API. 61 | * 62 | * @param msg ROS EnableDisable message to be consumed. 63 | */ 64 | void enableDisableCallback(const roscco::EnableDisable::ConstPtr& msg); 65 | 66 | private: 67 | ros::Subscriber topic_brake_command_; 68 | 69 | ros::Subscriber topic_steering_command_; 70 | 71 | ros::Subscriber topic_throttle_command_; 72 | 73 | ros::Subscriber topic_enable_disable_command_; 74 | }; 75 | 76 | #endif // ROS_TO_OSCC_H 77 | -------------------------------------------------------------------------------- /example/roscco_apollo.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define THROTTLE_RATIO 0.393 14 | #define STEERING_RATIO 0.018 15 | 16 | #if defined( KIA_SOUL_EV ) 17 | #define BRAKE_RATIO 0.12 18 | #define SPEED_RATIO 0.002 19 | #elif defined( KIA_NIRO ) 20 | #define BRAKE_RATIO 0.033 21 | #define SPEED_RATIO 0.3 22 | #endif 23 | 24 | class RosccoApollo 25 | { 26 | public: 27 | 28 | /** 29 | * @brief RosccoApollo class initializer 30 | * 31 | * This function construct a class which subscribes to Apollo messages and publishes ROSCCO messages 32 | */ 33 | RosccoApollo(); 34 | 35 | private: 36 | 37 | /** 38 | * @brief Callback function to pipe Apollo SteeringCommand to ROSCCO 39 | * 40 | * @param apollo control command message to be consumed 41 | */ 42 | void steeringCallback( const apollo::control::ControlCommand& input ); 43 | 44 | /** 45 | * @brief Callback function to pipe Apollo BrakeCommand to ROSCCO 46 | * 47 | * @param apollo control command message to be consumed 48 | */ 49 | void brakeCallback( const apollo::control::ControlCommand& input ); 50 | 51 | /** 52 | * @brief Callback function to pipe Apollo ThrottleCommand to ROSCCO 53 | * 54 | * @param apollo control command message to be consumed 55 | */ 56 | void throttleCallback( const apollo::control::ControlCommand& input ); 57 | 58 | /** 59 | * @brief Soul EV Callback function to save and publish chassis status 60 | * 61 | * @param roscco can frame message to be consumed 62 | */ 63 | void canFrameCallback( const roscco::CanFrame& input ); 64 | 65 | ros::NodeHandle nh; 66 | 67 | ros::Publisher throttle_pub; 68 | ros::Publisher brake_pub; 69 | ros::Publisher steering_pub; 70 | ros::Publisher chassis_pub; 71 | ros::Subscriber throttle_sub; 72 | ros::Subscriber brake_sub; 73 | ros::Subscriber steering_sub; 74 | ros::Subscriber can_frame_sub; 75 | ros::Subscriber localization_sub; 76 | 77 | double steering_angle_report = 0; 78 | double throttle_report = 0; 79 | double brake_report = 0; 80 | double speed_report = 0; 81 | }; 82 | 83 | 84 | /** 85 | * @brief setup a pid to control the steering angle 86 | * 87 | * @param setpoint/target 88 | * @param command 89 | * @param steering angle position 90 | */ 91 | void closedLoopControl( double setpoint, 92 | roscco::SteeringCommand& output, 93 | double steering_angle_report ); 94 | -------------------------------------------------------------------------------- /test/test_ros_to_oscc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include 21 | 22 | // Validates that publishing a brake command message yields the expected API 23 | // call to OSCC 24 | RC_GTEST_PROP(TestOSCCToROS, SendBrakeCommandMessage, ()) 25 | { 26 | ros::NodeHandle pub_nh; 27 | 28 | ros::Publisher pub = pub_nh.advertise("brake_command", 10); 29 | 30 | roscco::BrakeCommand* command(new roscco::BrakeCommand); 31 | 32 | command->brake_position = *rc::gen::arbitrary(); 33 | 34 | command->header.stamp = ros::Time::now(); 35 | 36 | pub.publish(*command); 37 | 38 | ros::WallDuration(0.1).sleep(); 39 | 40 | RC_ASSERT(get_brake_command() == command->brake_position); 41 | } 42 | 43 | // Validates that publishing a steering command message yields the expected API 44 | // call to OSCC 45 | RC_GTEST_PROP(TestOSCCToROS, SendSteeringCommandMessage, ()) 46 | { 47 | ros::NodeHandle pub_nh; 48 | 49 | ros::Publisher pub = pub_nh.advertise("steering_command", 10); 50 | 51 | roscco::SteeringCommand* command(new roscco::SteeringCommand); 52 | 53 | command->steering_torque = *rc::gen::arbitrary(); 54 | 55 | command->header.stamp = ros::Time::now(); 56 | 57 | pub.publish(*command); 58 | 59 | ros::WallDuration(0.1).sleep(); 60 | 61 | // TODO: Should there be bounds checking in ROS for these? 62 | RC_ASSERT(get_steering_torque() == command->steering_torque); 63 | } 64 | 65 | // Validates that publishing a steering command message yields the expected API 66 | // call to OSCC 67 | RC_GTEST_PROP(TestOSCCToROS, SendThrottleCommandMessage, ()) 68 | { 69 | ros::NodeHandle pub_nh; 70 | 71 | ros::Publisher pub = pub_nh.advertise("throttle_command", 10); 72 | 73 | roscco::ThrottleCommand* command(new roscco::ThrottleCommand); 74 | 75 | command->throttle_position = *rc::gen::arbitrary(); 76 | 77 | command->header.stamp = ros::Time::now(); 78 | 79 | pub.publish(*command); 80 | 81 | ros::WallDuration(0.1).sleep(); 82 | 83 | // TODO: Should there be bounds checking in ROS for these? 84 | RC_ASSERT(get_throttle_command() == command->throttle_position); 85 | } 86 | 87 | int main(int argc, char** argv) 88 | { 89 | testing::InitGoogleTest(&argc, argv); 90 | 91 | ros::init(argc, argv, "test_ros_to_oscc"); 92 | 93 | ros::NodeHandle public_nh, private_nh("~"); 94 | 95 | int ret = OSCC_ERROR; 96 | 97 | RosToOscc subcriber(&public_nh, &private_nh); 98 | 99 | ros::AsyncSpinner spinner(1); 100 | spinner.start(); 101 | 102 | ret = RUN_ALL_TESTS(); 103 | 104 | spinner.stop(); 105 | ros::shutdown(); 106 | 107 | return ret; 108 | } 109 | -------------------------------------------------------------------------------- /src/ros_to_oscc.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | RosToOscc::RosToOscc(ros::NodeHandle* public_nh, ros::NodeHandle* private_nh) 4 | { 5 | sigset_t mask; 6 | sigset_t orig_mask; 7 | 8 | sigemptyset(&mask); 9 | sigemptyset(&orig_mask); 10 | sigaddset(&mask, SIGIO); 11 | 12 | // Temporary block of OSCC SIGIO while initializing ROS publication to prevent 13 | // signal conflicts 14 | if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0) 15 | { 16 | ROS_ERROR("Failed to block SIGIO"); 17 | } 18 | 19 | topic_brake_command_ = 20 | public_nh->subscribe("brake_command", 10, &RosToOscc::brakeCommandCallback, this); 21 | 22 | topic_steering_command_ = 23 | public_nh->subscribe("steering_command", 10, &RosToOscc::steeringCommandCallback, this); 24 | 25 | topic_throttle_command_ = 26 | public_nh->subscribe("throttle_command", 10, &RosToOscc::throttleCommandCallback, this); 27 | 28 | topic_enable_disable_command_ = 29 | public_nh->subscribe("enable_disable", 10, &RosToOscc::enableDisableCallback, this); 30 | 31 | if (sigprocmask(SIG_SETMASK, &orig_mask, NULL) < 0) 32 | { 33 | ROS_ERROR("Failed to unblock SIGIO"); 34 | } 35 | }; 36 | 37 | void RosToOscc::brakeCommandCallback(const roscco::BrakeCommand::ConstPtr& msg) 38 | { 39 | oscc_result_t ret = OSCC_ERROR; 40 | 41 | ret = oscc_publish_brake_position(msg->brake_position); 42 | 43 | if (ret == OSCC_ERROR) 44 | { 45 | ROS_ERROR("OSCC_ERROR occured while trying send the brake position."); 46 | } 47 | else if (ret == OSCC_WARNING) 48 | { 49 | ROS_WARN("OSCC_WARNING occured while trying send the brake position."); 50 | } 51 | }; 52 | 53 | void RosToOscc::steeringCommandCallback(const roscco::SteeringCommand::ConstPtr& msg) 54 | { 55 | oscc_result_t ret = OSCC_ERROR; 56 | 57 | ret = oscc_publish_steering_torque(msg->steering_torque); 58 | 59 | if (ret == OSCC_ERROR) 60 | { 61 | ROS_ERROR("OSCC_ERROR occured while trying send the steering torque."); 62 | } 63 | else if (ret == OSCC_WARNING) 64 | { 65 | ROS_WARN("OSCC_WARNING occured while trying send the steering torque."); 66 | } 67 | }; 68 | 69 | void RosToOscc::throttleCommandCallback(const roscco::ThrottleCommand::ConstPtr& msg) 70 | { 71 | oscc_result_t ret = OSCC_ERROR; 72 | 73 | ret = oscc_publish_throttle_position(msg->throttle_position); 74 | 75 | if (ret == OSCC_ERROR) 76 | { 77 | ROS_ERROR("OSCC_ERROR occured while trying send the throttle position."); 78 | } 79 | else if (ret == OSCC_WARNING) 80 | { 81 | ROS_WARN("OSCC_WARNING occured while trying send the throttle position."); 82 | } 83 | }; 84 | 85 | void RosToOscc::enableDisableCallback(const roscco::EnableDisable::ConstPtr& msg) 86 | { 87 | oscc_result_t ret = OSCC_ERROR; 88 | 89 | ret = msg->enable_control ? oscc_enable() : oscc_disable(); 90 | 91 | if (ret == OSCC_ERROR) 92 | { 93 | ROS_ERROR("OSCC_ERROR occured while trying to enable or disable control."); 94 | } 95 | else if (ret == OSCC_WARNING) 96 | { 97 | ROS_WARN("OSCC_WARNING occured while trying to enable or disable control."); 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /src/oscc_to_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | OsccToRos::OsccToRos(ros::NodeHandle* public_nh, ros::NodeHandle* private_nh) 4 | { 5 | sigset_t mask; 6 | sigset_t orig_mask; 7 | 8 | sigemptyset(&mask); 9 | sigemptyset(&orig_mask); 10 | sigaddset(&mask, SIGIO); 11 | 12 | // Temporary block of OSCC SIGIO while initializing ROS subscription to 13 | // prevent signal conflicts 14 | if (sigprocmask(SIG_BLOCK, &mask, &orig_mask) < 0) 15 | { 16 | ROS_ERROR("Failed to block SIGIO"); 17 | } 18 | 19 | topic_brake_report_ = public_nh->advertise("brake_report", 10); 20 | 21 | topic_steering_report_ = public_nh->advertise("steering_report", 10); 22 | 23 | topic_throttle_report_ = public_nh->advertise("throttle_report", 10); 24 | 25 | topic_fault_report_ = public_nh->advertise("fault_report", 10); 26 | 27 | topic_obd_messages_ = public_nh->advertise("can_frame", 10); 28 | 29 | if (sigprocmask(SIG_SETMASK, &orig_mask, NULL) < 0) 30 | { 31 | ROS_ERROR("Failed to unblock SIGIO"); 32 | } 33 | 34 | oscc_subscribe_to_brake_reports(brake_callback); 35 | 36 | oscc_subscribe_to_steering_reports(steering_callback); 37 | 38 | oscc_subscribe_to_throttle_reports(throttle_callback); 39 | 40 | oscc_subscribe_to_fault_reports(fault_callback); 41 | 42 | oscc_subscribe_to_obd_messages(obd_callback); 43 | } 44 | 45 | void OsccToRos::steering_callback(oscc_steering_report_s* report) 46 | { 47 | cast_callback(report, 48 | &topic_steering_report_); 49 | } 50 | 51 | void OsccToRos::brake_callback(oscc_brake_report_s* report) 52 | { 53 | cast_callback(report, &topic_brake_report_); 54 | } 55 | 56 | void OsccToRos::throttle_callback(oscc_throttle_report_s* report) 57 | { 58 | cast_callback(report, 59 | &topic_throttle_report_); 60 | } 61 | 62 | template 63 | void OsccToRos::cast_callback(OSCCTYPE* report, ros::Publisher* pub) 64 | { 65 | ROSMSGTYPE* ros_message(new ROSMSGTYPE); 66 | 67 | ROSDATATYPE* data = (ROSDATATYPE*)report; 68 | 69 | ros_message->data = *data; 70 | 71 | ros_message->header.stamp = ros::Time::now(); 72 | 73 | pub->publish(*ros_message); 74 | 75 | delete ros_message; 76 | } 77 | 78 | void OsccToRos::fault_callback(oscc_fault_report_s* report) 79 | { 80 | roscco::FaultReport* ros_message(new roscco::FaultReport); 81 | 82 | // ROS does not pack the structs so individual assignment is required over 83 | // cast 84 | ros_message->data.magic[0] = report->magic[0]; 85 | ros_message->data.magic[1] = report->magic[1]; 86 | ros_message->data.fault_origin_id = report->fault_origin_id; 87 | ros_message->data.dtcs = report->dtcs; 88 | ros_message->data.reserved = report->reserved; 89 | 90 | ros_message->header.stamp = ros::Time::now(); 91 | 92 | topic_fault_report_.publish(*ros_message); 93 | 94 | delete ros_message; 95 | } 96 | 97 | void OsccToRos::obd_callback(can_frame* frame) 98 | { 99 | roscco::CanFrame* ros_message(new roscco::CanFrame); 100 | 101 | ros_message->frame.can_id = frame->can_id; 102 | 103 | ros_message->frame.can_dlc = frame->can_dlc; 104 | 105 | for(int i = 0; i < ros_message->frame.CAN_FRAME_DATA_MAX_SIZE; i ++) 106 | { 107 | ros_message->frame.data[i] = frame->data[i]; 108 | } 109 | 110 | ros_message->header.stamp = ros::Time::now(); 111 | 112 | topic_obd_messages_.publish(*ros_message); 113 | 114 | delete ros_message; 115 | } 116 | -------------------------------------------------------------------------------- /include/roscco/oscc_to_ros.h: -------------------------------------------------------------------------------- 1 | #ifndef OSCC_TO_ROS_H 2 | #define OSCC_TO_ROS_H 3 | 4 | #include 5 | 6 | extern "C" { 7 | #include 8 | } 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | static ros::Publisher topic_brake_report_; 23 | 24 | static ros::Publisher topic_steering_report_; 25 | 26 | static ros::Publisher topic_throttle_report_; 27 | 28 | static ros::Publisher topic_fault_report_; 29 | 30 | static ros::Publisher topic_obd_messages_; 31 | 32 | class OsccToRos 33 | { 34 | public: 35 | /** 36 | * @brief OsccToRos class initializer 37 | * 38 | * This function constructs ROS publishers and initializes the OSCC callbacks to begin transfering messages from OSCC 39 | * to ROS 40 | * 41 | * @param public_nh The public node handle to use for ROS publishers. 42 | * @param private_nh The private node handle for ROS parameters. 43 | */ 44 | OsccToRos(ros::NodeHandle* public_nh, ros::NodeHandle* private_nh); 45 | 46 | /** 47 | * @brief Callback function to publish oscc_steering_report messages on ROS. 48 | * 49 | * This function is a callback that consumes oscc_steering_report messages by utilizing the OSCC callback and 50 | * publishes that message with a ROS Publisher as a SteeringReport mesage. 51 | * 52 | * @param report The osc_steering_report message to be consumed by ROS. 53 | */ 54 | static void steering_callback(oscc_steering_report_s* report); 55 | 56 | /** 57 | * @brief Callback function to publish oscc_brake_report messages on ROS. 58 | * 59 | * This function is a callback that consumes oscc_brake_report messages by utilizing the OSCC callback and 60 | * publishes that message with a ROS Publisher as a BrakeReport message. 61 | * 62 | * @param report The oscc_brake_report message to be consumed by ROS. 63 | */ 64 | static void brake_callback(oscc_brake_report_s* report); 65 | 66 | /** 67 | * @brief Callback function to publish oscc_throttle_report messages on ROS. 68 | * 69 | * This function is a callback that consumes oscc_throttle_report messages by utilizing the OSCC callback and 70 | * publishes that message with a ROS Publisher as a ThrottleReport message. 71 | * 72 | * @param report The oscc_throttle_report message to be consumed by ROS. 73 | */ 74 | static void throttle_callback(oscc_throttle_report_s* report); 75 | 76 | /** 77 | * @brief Callback function to publish oscc_fault_report messages on ROS. 78 | * 79 | * This function is a callback that consumes oscc_fault_report messages by utilizing the OSCC callback and publishes 80 | * that message with a ROS Publisher as a FaultReport message. 81 | * 82 | * @param report The oscc_fault_report message to be consumed by ROS. 83 | */ 84 | static void fault_callback(oscc_fault_report_s* report); 85 | 86 | /** 87 | * @brief Callback function to publish can_frame messages on ROS. 88 | * 89 | * This function is a callback that consumes can_frame messages by utilizing the OSCC callback and publishes that 90 | * message with a ROS Publisher as a CanFrame message. 91 | * 92 | * @param report The oscc can_frame message to be consumed by ROS. 93 | */ 94 | static void obd_callback(can_frame* frame); 95 | 96 | /** 97 | * @brief Cast OSCC report to ROS message and publish 98 | * 99 | * This function casts an OSCC report type to a ros message and publishes the message onto ROS. Where the ROSMSGTYPE 100 | * is the type that gets published containing the ROS header and wraps the now casted ROSDATATYPE. The OSCCTYPE 101 | * is cast to the ROSDATATYPE which should be the same memory layout as the OSCCTYPE. 102 | * 103 | * Usage: 104 | * 105 | * cast an oscc_steering_report message to ROS and publish with the Publisher topic_steering_report_. 106 | * 107 | * cast_callback(report, &topic_steering_report_); 110 | * 111 | * @tparam OSCCTYPE OSCC report message data type 112 | * @tparam ROSMSGTYPE ROS parent message type 113 | * @tparam ROSDATATYPE ROS data message type 114 | * 115 | * @param report OSCC report message 116 | * @param pub ROS publisher to send the OSCC message 117 | */ 118 | template 119 | static void cast_callback(OSCCTYPE* report, ros::Publisher* pub); 120 | }; 121 | 122 | #endif // OSCC_TO_ROS_H 123 | -------------------------------------------------------------------------------- /example/roscco_apollo.cpp: -------------------------------------------------------------------------------- 1 | #include "roscco_apollo.h" 2 | 3 | 4 | RosccoApollo::RosccoApollo() 5 | { 6 | brake_pub = nh.advertise( "brake_command", 1 ); 7 | throttle_pub = nh.advertise( "throttle_command", 1 ); 8 | steering_pub = nh.advertise( "steering_command", 1 ); 9 | chassis_pub = nh.advertise( "/apollo/canbus/chassis", 1 ); 10 | 11 | steering_sub = nh.subscribe( "/apollo/control", 1, &RosccoApollo::steeringCallback, this ); 12 | brake_sub = nh.subscribe( "/apollo/control", 1, &RosccoApollo::brakeCallback, this ); 13 | throttle_sub = nh.subscribe( "/apollo/control", 1, &RosccoApollo::throttleCallback, this ); 14 | 15 | can_frame_sub = nh.subscribe( "/can_frame", 1, &RosccoApollo::canFrameCallback, this ); 16 | } 17 | 18 | 19 | void RosccoApollo::steeringCallback( const apollo::control::ControlCommand& input ) 20 | { 21 | roscco::SteeringCommand output; 22 | 23 | output.header.stamp = ros::Time::now(); 24 | closedLoopControl( input.steering_target(), output, steering_angle_report ); 25 | 26 | steering_pub.publish( output ); 27 | } 28 | 29 | 30 | void RosccoApollo::brakeCallback( const apollo::control::ControlCommand& input ) 31 | { 32 | roscco::BrakeCommand output; 33 | 34 | output.header.stamp = ros::Time::now(); 35 | output.brake_position = input.brake() / 100; 36 | 37 | brake_pub.publish( output ); 38 | } 39 | 40 | 41 | void RosccoApollo::throttleCallback( const apollo::control::ControlCommand& input ) 42 | { 43 | roscco::ThrottleCommand output; 44 | 45 | output.header.stamp = ros::Time::now(); 46 | output.throttle_position = input.throttle() / 100; 47 | 48 | throttle_pub.publish( output ); 49 | } 50 | 51 | 52 | void RosccoApollo::canFrameCallback( const roscco::CanFrame& input ) 53 | { 54 | switch( input.frame.can_id ) 55 | { 56 | case KIA_SOUL_OBD_THROTTLE_PRESSURE_CAN_ID: 57 | { 58 | #if defined( KIA_SOUL_EV ) 59 | throttle_report = input.frame.data[4]; 60 | #elif defined( KIA_NIRO ) 61 | throttle_report = input.frame.data[7]; 62 | #endif 63 | 64 | throttle_report = throttle_report * THROTTLE_RATIO; 65 | 66 | break; 67 | } 68 | case KIA_SOUL_OBD_BRAKE_PRESSURE_CAN_ID: 69 | { 70 | #if defined( KIA_SOUL_EV ) 71 | brake_report = input.frame.data[4] + input.frame.data[5] * 256; 72 | #elif defined( KIA_NIRO ) 73 | brake_report = input.frame.data[3] + input.frame.data[4] * 256; 74 | #endif 75 | 76 | brake_report = brake_report * BRAKE_RATIO; 77 | 78 | break; 79 | } 80 | case KIA_SOUL_OBD_STEERING_WHEEL_ANGLE_CAN_ID: 81 | { 82 | steering_angle_report = input.frame.data[0] + input.frame.data[1] * 256; 83 | 84 | if( steering_angle_report > 60000 ) 85 | { 86 | steering_angle_report -= 65535; 87 | } 88 | 89 | steering_angle_report = steering_angle_report * STEERING_RATIO; 90 | 91 | break; 92 | } 93 | case KIA_SOUL_OBD_SPEED_CAN_ID: 94 | { 95 | #if defined( KIA_SOUL_EV ) 96 | speed_report = input.frame.data[3] + input.frame.data[2] * 128; 97 | 98 | speed_report = speed_report * SPEED_RATIO; 99 | 100 | #elif defined( KIA_NIRO ) 101 | speed_report = input.frame.data[0]; 102 | 103 | speed_report = speed_report * SPEED_RATIO; 104 | 105 | #endif 106 | 107 | break; 108 | } 109 | default : 110 | { 111 | /// Empty default to leave no untested cases 112 | break; 113 | } 114 | } 115 | 116 | apollo::canbus::Chassis output; 117 | 118 | output.set_steering_percentage( steering_angle_report ); 119 | output.set_throttle_percentage( throttle_report ); 120 | output.set_brake_percentage( brake_report ); 121 | output.set_speed_mps( speed_report ); 122 | 123 | chassis_pub.publish( output ); 124 | } 125 | 126 | 127 | void closedLoopControl( double setpoint, 128 | roscco::SteeringCommand& output, 129 | double steering_angle_report ) 130 | { 131 | pid_terms params; 132 | pid_state state; 133 | 134 | createPIDState( setpoint, &state ); 135 | 136 | params.max = 1; 137 | params.min = -1; 138 | params.p_term = 0.016; 139 | params.i_term = 0.004; 140 | params.d_term = 0.001; 141 | params.i_max = 250; 142 | 143 | output.steering_torque = pidController( ¶ms, &state, steering_angle_report ); 144 | } 145 | 146 | 147 | int main( int argc, char** argv ) 148 | { 149 | ros::init( argc, argv, "roscco_apollo" ); 150 | RosccoApollo roscco_apollo; 151 | 152 | ros::spin(); 153 | } 154 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROSCCO 2 | 3 | ## Overview 4 | 5 | ROSCCO is an interface between ROS and OSCC allowing ROS users to interface with 6 | OSCC from their own ROS nodes utilizing ROS messages created from the OSCC API. 7 | 8 | ## Getting Started 9 | 10 | ### Dependencies 11 | 12 | ROSCCO is built on the Linux implementation of ROS Kinetic. The only known 13 | limitation for using older versions is that the tests require C++11. 14 | 15 | To install ROS Kinetic see the 16 | [install instructions](http://wiki.ros.org/kinetic/Installation) on your Linux 17 | distribution. 18 | 19 | ### Building ROSCCO in a catkin project 20 | 21 | ROSCCO uses the OSCC API which builds for specific vehicle models. OSCC is a 22 | git submodule so that it can be built during the catkin build process. Make sure 23 | the version of ROSCCO you're using matches with the version of OSCC firmware you 24 | are using. See the [release page](https://github.com/PolySync/roscco/releases) 25 | for information about what releases ROSCCO points to. 26 | 27 | ``` 28 | mkdir -p catkin_ws/src && cd catkin_ws/src 29 | git clone --recursive https://github.com/PolySync/roscco.git 30 | cd .. 31 | source /opt/ros/kinetic/setup.bash 32 | catkin_make -DKIA_SOUL=ON 33 | ``` 34 | 35 | ## Usage 36 | 37 | ### Game Controller Example 38 | 39 | ROSCCO comes with a joystick message conversion example to work with. To make 40 | use of this, build the project with example turned on from your `catkin_ws` 41 | directory: 42 | 43 | ``` 44 | catkin_make -DKIA_SOUL=ON -DEXAMPLE=ON 45 | ``` 46 | 47 | This example has been used with a Logitech F310 and an Xbox 360 controller 48 | before, make sure that your controller maps accordingly. If it is not mapped 49 | correctly you'll want to make the required changes to the launch file and 50 | possibly the example code. More information on the joystick node can be found 51 | at: http://wiki.ros.org/joy 52 | 53 | To use the example you'll need to bring up a CAN connection for OSCC to 54 | communicate, source the newly compiled package and launch the three nodes, 55 | joy_node, roscco_teleop, and roscco_node. 56 | 57 | To install joy node 58 | ``` 59 | sudo apt-get install ros-kinetic-joy 60 | ``` 61 | 62 | `joy_node` uses `/dev/js0` by default to change this see the 63 | [joystick documentation](http://wiki.ros.org/joy/Tutorials/ConfiguringALinuxJoystick). 64 | 65 | ``` 66 | sudo ip link set can0 type can bitrate 500000 67 | sudo ip link set up can0 68 | source devel/setup.bash 69 | roslaunch src/roscco/example/example.launch 70 | ``` 71 | 72 | The roscco_teleop converts the joy messages from the joy_node into messages 73 | for roscco_node and the roscco_node sends it's received messages to OSCC API. 74 | 75 | ### Apollo open autonomous driving platform 76 | 77 | ROSCCO can serve as a bridge to Baidu's open autonomous driving platform Apollo. 78 | 79 | This example only support the Kia Soul EV. We are planning on extending support to the Kia Niro. 80 | You will need a double channel Kvaser or two single channel Kvaser, to connect to drivekit CAN and diagnostics CAN. 81 | 82 | #### Installation and build 83 | 84 | Install and build Apollo 85 | 86 | This project was developed on Apollo v2.0.0. It would most probably build on other version of Apollo with some modifications. 87 | 88 | ``` 89 | git clone https://github.com/ApolloAuto/apollo.git --branch v2.0.0 90 | cd apollo/ 91 | ./docker/scripts/dev_start.sh 92 | ./docker/scripts/dev_into.sh 93 | ./apollo.sh build 94 | ``` 95 | 96 | 97 | Install and build ROSCCO 98 | 99 | The installation instructions are very similar to the one above, the main difference is that everything needs to be done within Apollo's docker environment. 100 | 101 | ``` 102 | mkdir -p catkin_ws/src && cd catkin_ws/src 103 | git clone --recursive https://github.com/PolySync/roscco.git 104 | cd .. 105 | catkin_make -DCATKIN_ENABLE_TESTING=0 -DKIA_SOUL_EV=ON -DAPOLLO=ON 106 | source devel/setup.bash 107 | ``` 108 | 109 | #### Running ROSCCO with Apollo 110 | 111 | With Apollo control module running, 112 | 113 | ``` 114 | sudo ip link set can0 type can bitrate 500000 115 | sudo ip link set up can0 116 | sudo ip link set can1 type can bitrate 500000 117 | sudo ip link set up can1 118 | roslaunch roscco apollo.launch 119 | ``` 120 | 121 | You can enable OSCC using `rostopic pub`, 122 | 123 | ``` 124 | rostopic pub /enable_disable roscco/EnableDisable "header: 125 | seq: 0 126 | stamp: now 127 | frame_id: '' 128 | enable_control: true" 129 | ``` 130 | 131 | 132 | ## Running ROSCCO 133 | 134 | The default CAN channel is `can0` but can be specified as a node parameter and 135 | assumes there is a can device available through linux can. The best way to 136 | validate that is through `ifconfig | grep can` 137 | 138 | ``` 139 | sudo ip link set can0 type can bitrate 500000 140 | sudo ip link set up can0 141 | roscore & 142 | rosrun roscco roscco_node _can_channel=0 143 | ``` 144 | 145 | Passing a message through ROS should yield results on your OSCC device such as 146 | enabling control and turning the steering right with 20% torque: 147 | ``` 148 | rostopic pub /enable_disable roscco/EnableDisable -1 \ 149 | '{header: {stamp: now}, enable_control: true}' 150 | 151 | rostopic pub /steering_command roscco/SteeringCommand -1 \ 152 | '{header: {stamp: now}, steering_torque: 0.2}' 153 | ``` 154 | 155 | Similarly to the OSCC API, the Reports and CanFrame messages can be 156 | subscribed to in order to receive feedback from OSCC. The Command and 157 | EnableDisable messages will call the commands for the OSCC API. 158 | 159 | OSCC modules will disable if no messages are received within 200 ms. It is 160 | recommended to publish ROSCCO messages, at a rate of 20 Hz (every 50 ms) to 161 | ensure OSCC modules do not disable. 162 | 163 | ## Tests 164 | 165 | ## Test Dependencies 166 | 167 | ROSCCO requires GoogleTest to be 168 | [installed](https://github.com/google/googletest/blob/master/googletest/README.md). 169 | 170 | ROSCCO also depends on RapidCheck which is included as a submodule, ensure that 171 | the repository has been cloned recursively to include all submodules. 172 | 173 | ``` 174 | git submodule update --init --recursive 175 | ``` 176 | 177 | ## Testing ROSCCO build 178 | 179 | If you have already cloned the repository and have build ROSCCO you should be 180 | able to compile and run the tests as well after you `cd` into your `catkin_ws` 181 | directory: 182 | 183 | ``` 184 | catkin_make run_tests -DKIA_SOUL=ON 185 | ``` 186 | 187 | If you have not cloned the repository yet: 188 | 189 | ``` 190 | mkdir -p catkin_ws/src && cd catkin_ws/src 191 | git clone --recursive https://github.com/PolySync/roscco.git 192 | cd .. 193 | source /opt/ros/kinetic/setup.bash 194 | catkin_make -DKIA_SOUL=ON 195 | catkin_make run_tests -DKIA_SOUL=ON 196 | ``` 197 | 198 | # License 199 | 200 | ROSCCO is licensed under the MIT License (MIT) unless otherwise noted 201 | (e.g. 3rd party dependencies, etc.). 202 | 203 | Please see [LICENSE.md](LICENSE.md) for more details. 204 | -------------------------------------------------------------------------------- /test/test_oscc_to_ros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | 9 | // Time to allow ROS to process callbacks and publish a message 10 | const double SLEEP_TIME = 0.05; 11 | 12 | template 13 | class MessageHelper 14 | { 15 | public: 16 | MessageHelper() : count(0) 17 | { 18 | } 19 | 20 | void cb(const T& msg) 21 | { 22 | ++count; 23 | message = &msg; 24 | } 25 | 26 | const T* message; 27 | uint32_t count; 28 | }; 29 | 30 | RC_GTEST_PROP(TestOSCCToROS, CastBrakeReport, ()) 31 | { 32 | ros::NodeHandle sub_nh; 33 | MessageHelper h; 34 | ros::Subscriber sub = sub_nh.subscribe("brake_report", 0, &MessageHelper::cb, &h); 35 | 36 | oscc_brake_report_s* data = new oscc_brake_report_s; 37 | 38 | data->magic[0] = *rc::gen::arbitrary(); 39 | data->magic[1] = *rc::gen::arbitrary(); 40 | data->enabled = *rc::gen::arbitrary(); 41 | data->operator_override = *rc::gen::arbitrary(); 42 | data->dtcs = *rc::gen::arbitrary(); 43 | data->reserved[0] = *rc::gen::arbitrary(); 44 | data->reserved[1] = *rc::gen::arbitrary(); 45 | data->reserved[2] = *rc::gen::arbitrary(); 46 | 47 | brake_report_callback(data); 48 | 49 | ros::WallDuration(SLEEP_TIME).sleep(); 50 | RC_ASSERT(h.count > 0); 51 | RC_ASSERT(h.message->data.magic[0] == data->magic[0]); 52 | RC_ASSERT(h.message->data.magic[1] == data->magic[1]); 53 | RC_ASSERT(h.message->data.enabled == data->enabled); 54 | RC_ASSERT(h.message->data.operator_override == data->operator_override); 55 | RC_ASSERT(h.message->data.dtcs == data->dtcs); 56 | RC_ASSERT(h.message->data.reserved[0] == data->reserved[0]); 57 | RC_ASSERT(h.message->data.reserved[1] == data->reserved[1]); 58 | RC_ASSERT(h.message->data.reserved[2] == data->reserved[2]); 59 | 60 | delete data; 61 | } 62 | 63 | RC_GTEST_PROP(TestOSCCToROS, CastThrottleReport, ()) 64 | { 65 | ros::NodeHandle sub_nh; 66 | MessageHelper h; 67 | ros::Subscriber sub = sub_nh.subscribe("throttle_report", 0, &MessageHelper::cb, &h); 68 | 69 | oscc_throttle_report_s* data = new oscc_throttle_report_s; 70 | 71 | data->magic[0] = *rc::gen::arbitrary(); 72 | data->magic[1] = *rc::gen::arbitrary(); 73 | data->enabled = *rc::gen::arbitrary(); 74 | data->operator_override = *rc::gen::arbitrary(); 75 | data->dtcs = *rc::gen::arbitrary(); 76 | data->reserved[0] = *rc::gen::arbitrary(); 77 | data->reserved[1] = *rc::gen::arbitrary(); 78 | data->reserved[2] = *rc::gen::arbitrary(); 79 | 80 | throttle_report_callback(data); 81 | 82 | ros::WallDuration(SLEEP_TIME).sleep(); 83 | RC_ASSERT(h.count > 0); 84 | RC_ASSERT(h.message->data.magic[0] == data->magic[0]); 85 | RC_ASSERT(h.message->data.magic[1] == data->magic[1]); 86 | RC_ASSERT(h.message->data.enabled == data->enabled); 87 | RC_ASSERT(h.message->data.operator_override == data->operator_override); 88 | RC_ASSERT(h.message->data.dtcs == data->dtcs); 89 | RC_ASSERT(h.message->data.reserved[0] == data->reserved[0]); 90 | RC_ASSERT(h.message->data.reserved[1] == data->reserved[1]); 91 | RC_ASSERT(h.message->data.reserved[2] == data->reserved[2]); 92 | 93 | delete data; 94 | } 95 | 96 | RC_GTEST_PROP(TestOSCCToROS, CastSteeringReport, ()) 97 | { 98 | ros::NodeHandle sub_nh; 99 | MessageHelper h; 100 | ros::Subscriber sub = sub_nh.subscribe("steering_report", 0, &MessageHelper::cb, &h); 101 | 102 | oscc_steering_report_s* data = new oscc_steering_report_s; 103 | 104 | data->magic[0] = *rc::gen::arbitrary(); 105 | data->magic[1] = *rc::gen::arbitrary(); 106 | data->enabled = *rc::gen::arbitrary(); 107 | data->operator_override = *rc::gen::arbitrary(); 108 | data->dtcs = *rc::gen::arbitrary(); 109 | data->reserved[0] = *rc::gen::arbitrary(); 110 | data->reserved[1] = *rc::gen::arbitrary(); 111 | data->reserved[2] = *rc::gen::arbitrary(); 112 | 113 | steering_report_callback(data); 114 | 115 | ros::WallDuration(SLEEP_TIME).sleep(); 116 | RC_ASSERT(h.count > 0); 117 | RC_ASSERT(h.message->data.magic[0] == data->magic[0]); 118 | RC_ASSERT(h.message->data.magic[1] == data->magic[1]); 119 | RC_ASSERT(h.message->data.enabled == data->enabled); 120 | RC_ASSERT(h.message->data.operator_override == data->operator_override); 121 | RC_ASSERT(h.message->data.dtcs == data->dtcs); 122 | RC_ASSERT(h.message->data.reserved[0] == data->reserved[0]); 123 | RC_ASSERT(h.message->data.reserved[1] == data->reserved[1]); 124 | RC_ASSERT(h.message->data.reserved[2] == data->reserved[2]); 125 | 126 | delete data; 127 | } 128 | 129 | RC_GTEST_PROP(TESTOSCCToROS, CastFaultReport, ()) 130 | { 131 | ros::NodeHandle sub_nh; 132 | MessageHelper h; 133 | ros::Subscriber sub = sub_nh.subscribe("fault_report", 0, &MessageHelper::cb, &h); 134 | 135 | oscc_fault_report_s* data = new oscc_fault_report_s; 136 | 137 | data->magic[0] = *rc::gen::arbitrary(); 138 | data->magic[1] = *rc::gen::arbitrary(); 139 | data->fault_origin_id = *rc::gen::arbitrary(); 140 | data->dtcs = *rc::gen::arbitrary(); 141 | data->reserved = *rc::gen::arbitrary(); 142 | 143 | fault_report_callback(data); 144 | 145 | ros::WallDuration(SLEEP_TIME).sleep(); 146 | 147 | RC_ASSERT(h.count > 0); 148 | RC_ASSERT(h.message->data.magic[0] == data->magic[0]); 149 | RC_ASSERT(h.message->data.magic[1] == data->magic[1]); 150 | RC_ASSERT(h.message->data.fault_origin_id == data->fault_origin_id); 151 | RC_ASSERT(h.message->data.dtcs == data->dtcs); 152 | RC_ASSERT(h.message->data.reserved == data->reserved); 153 | 154 | delete data; 155 | } 156 | 157 | RC_GTEST_PROP(TESTOSCCToROS, CastOBDFrame, ()) 158 | { 159 | ros::NodeHandle sub_nh; 160 | MessageHelper h; 161 | ros::Subscriber sub = sub_nh.subscribe("can_frame", 0, &MessageHelper::cb, &h); 162 | 163 | can_frame* frame = new can_frame; 164 | 165 | frame->can_id = *rc::gen::arbitrary(); 166 | frame->can_dlc = 8; 167 | 168 | for (int i = 0; i < frame->can_dlc; i++) 169 | { 170 | frame->data[i] = *rc::gen::arbitrary(); 171 | } 172 | 173 | obd_frame_callback(frame); 174 | 175 | ros::WallDuration(SLEEP_TIME).sleep(); 176 | 177 | RC_ASSERT(h.count > 0); 178 | 179 | RC_ASSERT(h.message->frame.can_id == frame->can_id); 180 | RC_ASSERT(h.message->frame.can_dlc == frame->can_dlc); 181 | 182 | for (int i = 0; i < frame->can_dlc; i++) 183 | { 184 | RC_ASSERT(h.message->frame.data[i] == frame->data[i]); 185 | } 186 | 187 | delete frame; 188 | } 189 | 190 | int main(int argc, char** argv) 191 | { 192 | testing::InitGoogleTest(&argc, argv); 193 | 194 | ros::init(argc, argv, "test_oscc_to_ros"); 195 | 196 | ros::NodeHandle n, nh("~"); 197 | 198 | OsccToRos oscc_publisher(&n, &nh); 199 | 200 | ros::AsyncSpinner spinner(1); 201 | spinner.start(); 202 | 203 | int ret = RUN_ALL_TESTS(); 204 | 205 | spinner.stop(); 206 | ros::shutdown(); 207 | return ret; 208 | } 209 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roscco) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | ## C++11 is required for rapidcheck 6 | if (CMAKE_VERSION VERSION_LESS "3.1") 7 | if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU") 8 | set (CMAKE_CXX_FLAGS "-std=gnu++11 ${CMAKE_CXX_FLAGS}") 9 | endif () 10 | else () 11 | set (CMAKE_CXX_STANDARD 11) 12 | endif () 13 | 14 | include(oscc/api/OsccConfig.cmake) 15 | 16 | find_package(catkin REQUIRED COMPONENTS 17 | message_generation 18 | roscpp 19 | std_msgs 20 | ) 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | add_message_files( 27 | FILES 28 | BrakeCommand.msg 29 | BrakeReport.msg 30 | BrakeReportData.msg 31 | CanFrame.msg 32 | CanFrameData.msg 33 | EnableDisable.msg 34 | FaultReport.msg 35 | FaultReportData.msg 36 | SteeringCommand.msg 37 | SteeringReport.msg 38 | SteeringReportData.msg 39 | ThrottleCommand.msg 40 | ThrottleReport.msg 41 | ThrottleReportData.msg 42 | ) 43 | 44 | generate_messages( 45 | DEPENDENCIES 46 | std_msgs 47 | ) 48 | 49 | 50 | ################################### 51 | ## catkin specific configuration ## 52 | ################################### 53 | catkin_package( 54 | LIBRARIES oscc_api ${PROJECT_NAME}_ros_to_oscc ${PROJECT_NAME}_oscc_to_ros 55 | CATKIN_DEPENDS message_runtime roscpp std_msgs 56 | ) 57 | 58 | ########### 59 | ## Build ## 60 | ########### 61 | 62 | include_directories(${catkin_INCLUDE_DIRS}) 63 | 64 | add_library( 65 | oscc_api 66 | oscc/api/src/oscc.c 67 | ) 68 | 69 | target_include_directories( 70 | oscc_api PUBLIC 71 | include 72 | oscc/api/include 73 | ) 74 | 75 | add_library( 76 | ${PROJECT_NAME}_ros_to_oscc 77 | src/ros_to_oscc.cpp 78 | ) 79 | 80 | target_include_directories( 81 | ${PROJECT_NAME}_ros_to_oscc PUBLIC 82 | include 83 | oscc/api/include 84 | ${catkin_INCLUDE_DIRS} 85 | ) 86 | 87 | add_library( 88 | ${PROJECT_NAME}_oscc_to_ros 89 | src/oscc_to_ros.cpp 90 | ) 91 | 92 | target_include_directories( 93 | ${PROJECT_NAME}_oscc_to_ros PUBLIC 94 | include 95 | oscc/api/include 96 | ) 97 | 98 | target_link_libraries( 99 | ${PROJECT_NAME}_oscc_to_ros 100 | oscc_api 101 | ) 102 | 103 | target_link_libraries( 104 | ${PROJECT_NAME}_ros_to_oscc 105 | oscc_api 106 | ) 107 | 108 | add_dependencies( 109 | ${PROJECT_NAME}_ros_to_oscc 110 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 111 | ${catkin_EXPORTED_TARGETS} 112 | ) 113 | 114 | add_dependencies( 115 | ${PROJECT_NAME}_oscc_to_ros 116 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 117 | ${catkin_EXPORTED_TARGETS} 118 | ) 119 | 120 | add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) 121 | 122 | target_include_directories( 123 | ${PROJECT_NAME}_node PUBLIC 124 | include 125 | oscc/api/include 126 | ) 127 | 128 | add_dependencies( 129 | ${PROJECT_NAME}_node 130 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 131 | ${catkin_EXPORTED_TARGETS} 132 | ) 133 | 134 | ## Specify libraries to link a library or executable target against 135 | target_link_libraries(${PROJECT_NAME}_node 136 | oscc_api 137 | ${PROJECT_NAME}_ros_to_oscc 138 | ${PROJECT_NAME}_oscc_to_ros 139 | ${catkin_LIBRARIES} 140 | ) 141 | 142 | if(EXAMPLE) 143 | 144 | add_executable( 145 | ${PROJECT_NAME}_teleop 146 | example/${PROJECT_NAME}_teleop.cpp 147 | ) 148 | 149 | target_include_directories( 150 | ${PROJECT_NAME}_teleop PUBLIC 151 | include 152 | ) 153 | 154 | add_dependencies( 155 | ${PROJECT_NAME}_teleop 156 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 157 | ${catkin_EXPORTED_TARGETS} 158 | ) 159 | 160 | target_link_libraries( 161 | ${PROJECT_NAME}_teleop 162 | ${catkin_LIBRARIES} 163 | ) 164 | 165 | endif() 166 | 167 | if(APOLLO AND (KIA_SOUL_EV OR KIA_NIRO)) 168 | 169 | find_package(catkin 170 | REQUIRED COMPONENTS 171 | roscpp 172 | rospy 173 | std_msgs 174 | genmsg) 175 | 176 | include_directories(include 177 | ${catkin_INCLUDE_DIRS} 178 | /apollo/bazel-apollo/bazel-out/local-dbg/genfiles/ 179 | ) 180 | 181 | add_library( 182 | ${PROJECT_NAME}_pid_control 183 | src/pid_control.cpp 184 | ) 185 | 186 | 187 | link_directories( 188 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/canbus/ 189 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/control/ 190 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/common/proto 191 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/drivers/canbus/common/ 192 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/drivers/canbus/proto/ 193 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/drivers/canbus/ ) 194 | 195 | add_executable( 196 | ${PROJECT_NAME}_apollo 197 | example/${PROJECT_NAME}_apollo.cpp 198 | ) 199 | 200 | target_include_directories( 201 | ${PROJECT_NAME}_apollo PUBLIC 202 | include 203 | oscc/api/include 204 | ) 205 | 206 | add_dependencies( 207 | ${PROJECT_NAME}_apollo 208 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 209 | ${catkin_EXPORTED_TARGETS} 210 | ${PROJECT_NAME}_apollo_generate_messages_cpp 211 | ) 212 | 213 | target_link_libraries( 214 | ${PROJECT_NAME}_apollo 215 | ${catkin_LIBRARIES} 216 | ${PROJECT_NAME}_pid_control 217 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/canbus/proto/libcanbus_proto_lib.a 218 | /apollo/bazel-apollo/bazel-out/local-dbg/bin/modules/control/proto/libcontrol_proto_lib.a 219 | canbus_proto_lib 220 | protobuf 221 | canbus_lib 222 | control_lib 223 | canbus_common 224 | vehicle_signal_proto_lib 225 | pnc_point_proto_lib 226 | gnss_status_proto_lib 227 | car_sound_proto_lib 228 | header_proto_lib 229 | common_proto_lib 230 | error_code_proto_lib 231 | vehicle_state_proto_lib 232 | ) 233 | 234 | endif() 235 | 236 | ############# 237 | ## Testing ## 238 | ############# 239 | 240 | if(CATKIN_ENABLE_TESTING) 241 | 242 | find_package(rostest REQUIRED) 243 | 244 | add_subdirectory(test/rapidcheck) 245 | 246 | add_library( 247 | oscc_fixture 248 | test/oscc_fixture.c 249 | ) 250 | 251 | 252 | target_include_directories( 253 | oscc_fixture PUBLIC 254 | test/include 255 | oscc/api/include/can_protocols 256 | ) 257 | 258 | add_rostest_gtest( 259 | test_ros_oscc_api 260 | test/test_ros_to_oscc.launch 261 | test/test_ros_to_oscc.cpp 262 | ) 263 | 264 | target_include_directories( 265 | test_ros_oscc_api PUBLIC 266 | test/include 267 | test/rapidcheck/include 268 | test/rapidcheck/extras/gtest/include 269 | ) 270 | 271 | target_link_libraries( 272 | test_ros_oscc_api 273 | rapidcheck 274 | ${PROJECT_NAME}_ros_to_oscc 275 | oscc_fixture 276 | ${catkin_LIBRARIES} 277 | ) 278 | 279 | add_rostest_gtest( 280 | test_oscc_ros_api 281 | test/test_oscc_to_ros.launch 282 | test/test_oscc_to_ros.cpp 283 | ) 284 | 285 | target_include_directories( 286 | test_oscc_ros_api PUBLIC 287 | include 288 | test/include 289 | test/rapidcheck/include 290 | test/rapidcheck/extras/gtest/include 291 | oscc/api/include/can_protocols 292 | ) 293 | 294 | target_link_libraries( 295 | test_oscc_ros_api 296 | rapidcheck 297 | ${PROJECT_NAME}_oscc_to_ros 298 | oscc_fixture 299 | ${catkin_LIBRARIES} 300 | ) 301 | 302 | endif() 303 | 304 | ############# 305 | ## Install ## 306 | ############# 307 | 308 | install( 309 | TARGETS 310 | oscc_api 311 | ${PROJECT_NAME}_node 312 | ${PROJECT_NAME}_oscc_to_ros 313 | ${PROJECT_NAME}_ros_to_oscc 314 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 315 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 316 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 317 | ) 318 | 319 | install( 320 | DIRECTORY 321 | include/${PROJECT_NAME} 322 | oscc/api/include 323 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 324 | FILES_MATCHING PATTERN "*.h" 325 | ) 326 | -------------------------------------------------------------------------------- /example/roscco_teleop.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | double calc_exponential_average(double AVERAGE, double SETPOINT, double FACTOR); 10 | double linear_tranformation(double VALUE, double HIGH_1, double LOW_1, double HIGH_2, double LOW_2); 11 | 12 | class RosccoTeleop 13 | { 14 | public: 15 | RosccoTeleop(); 16 | 17 | private: 18 | void joystickCallback(const sensor_msgs::Joy::ConstPtr& joy); 19 | 20 | ros::NodeHandle nh_; 21 | 22 | ros::Publisher throttle_pub_; 23 | ros::Publisher brake_pub_; 24 | ros::Publisher steering_pub_; 25 | ros::Publisher enable_disable_pub_; 26 | ros::Subscriber joy_sub_; 27 | 28 | int previous_start_state_ = 0; 29 | int previous_back_state_ = 0; 30 | 31 | // Number of messages to retain when the message queue is full 32 | const int QUEUE_SIZE_ = 10; 33 | 34 | // Timed callback frequency set to OSCC recommended publishing rate of 20 Hz (50 ms == 0.05 s) 35 | const float CALLBACK_FREQ_ = 0.05; // Units in Seconds 36 | 37 | // OSCC input range 38 | const double BRAKE_MAX_ = 1; 39 | const double BRAKE_MIN_ = 0; 40 | const double THROTTLE_MAX_ = 1; 41 | const double THROTTLE_MIN_ = 0; 42 | const double STEERING_MAX_ = 1; 43 | const double STEERING_MIN_ = -1; 44 | 45 | // Store last known value for timed callback 46 | double brake_ = 0.0; 47 | double throttle_ = 0.0; 48 | double steering_ = 0.0; 49 | bool enabled_ = false; 50 | 51 | // Smooth the steering to remove twitchy joystick movements 52 | const double DATA_SMOOTHING_FACTOR_ = 0.1; 53 | double steering_average_ = 0.0; 54 | 55 | // Variable to ensure joystick triggers have been initialized 56 | bool initialized_ = false; 57 | 58 | // The threshold for considering the controller triggers to be parked in the correct position 59 | const double PARKED_THRESHOLD_ = 0.99; 60 | 61 | const int BRAKE_AXES_ = 2; 62 | const int THROTTLE_AXES_ = 5; 63 | const int STEERING_AXES_ = 0; 64 | const int START_BUTTON_ = 7; 65 | const int BACK_BUTTON_ = 6; 66 | 67 | const double TRIGGER_MIN_ = 1; 68 | const double TRIGGER_MAX_ = -1; 69 | const double JOYSTICK_MIN_ = 1; 70 | const double JOYSTICK_MAX_ = -1; 71 | }; 72 | 73 | /** 74 | * @brief ROSCCOTeleop class initializer 75 | * 76 | * This function constructs a class which subscribes to ROS Joystick messages, converts the inputs to ROSCCO relevant 77 | * values and publishes ROSCCO messages on a 20 Hz cadence. 78 | */ 79 | RosccoTeleop::RosccoTeleop() 80 | { 81 | brake_pub_ = nh_.advertise("brake_command", QUEUE_SIZE_); 82 | throttle_pub_ = nh_.advertise("throttle_command", QUEUE_SIZE_); 83 | steering_pub_ = nh_.advertise("steering_command", QUEUE_SIZE_); 84 | enable_disable_pub_ = nh_.advertise("enable_disable", QUEUE_SIZE_); 85 | 86 | joy_sub_ = nh_.subscribe("joy", QUEUE_SIZE_, &RosccoTeleop::joystickCallback, this); 87 | } 88 | 89 | /** 90 | * @brief Callback function consume a joystick message and map values to ROSCCO ranges 91 | * 92 | * This function consumes a joystick message and maps the joystick inputs to ROSCCO ranges which are stored to private 93 | * class variables. Since the Joystick messages are published before all buttons are initialized this function also 94 | * validates that the button ranges are valid before consuming the date. 95 | * 96 | * @param joy The ROS Joystick message to be consumed. 97 | */ 98 | void RosccoTeleop::joystickCallback(const sensor_msgs::Joy::ConstPtr& joy) 99 | { 100 | // gamepad triggers default 0 prior to using them which is 50% for the logitech and xbox controller the initilization 101 | // is to ensure the triggers have been pulled prior to enabling OSCC command 102 | if (initialized_) 103 | { 104 | // Map the trigger values [1, -1] to oscc values [0, 1] 105 | brake_ = linear_tranformation(joy->axes[BRAKE_AXES_], TRIGGER_MAX_, TRIGGER_MIN_, BRAKE_MAX_, BRAKE_MIN_); 106 | throttle_ = 107 | linear_tranformation(joy->axes[THROTTLE_AXES_], TRIGGER_MAX_, TRIGGER_MIN_, THROTTLE_MAX_, THROTTLE_MIN_); 108 | 109 | // Map the joystick to steering [1, -1] to oscc values [-1, 1] 110 | steering_ = 111 | linear_tranformation(joy->axes[STEERING_AXES_], JOYSTICK_MAX_, JOYSTICK_MIN_, STEERING_MAX_, STEERING_MIN_); 112 | 113 | roscco::EnableDisable enable_msg; 114 | enable_msg.header.stamp = ros::Time::now(); 115 | 116 | if ((previous_back_state_ == 0) && joy->buttons[BACK_BUTTON_]) 117 | { 118 | enable_msg.enable_control = false; 119 | enable_disable_pub_.publish(enable_msg); 120 | enabled_ = false; 121 | } 122 | else if ((previous_start_state_ == 0) && joy->buttons[START_BUTTON_]) 123 | { 124 | enable_msg.enable_control = true; 125 | enable_disable_pub_.publish(enable_msg); 126 | enabled_ = true; 127 | } 128 | 129 | previous_back_state_ = joy->buttons[BACK_BUTTON_]; 130 | previous_start_state_ = joy->buttons[START_BUTTON_]; 131 | 132 | if (enabled_) 133 | { 134 | roscco::BrakeCommand brake_msg; 135 | brake_msg.header.stamp = ros::Time::now(); 136 | brake_msg.brake_position = brake_; 137 | brake_pub_.publish(brake_msg); 138 | 139 | roscco::ThrottleCommand throttle_msg; 140 | throttle_msg.header.stamp = ros::Time::now(); 141 | throttle_msg.throttle_position = throttle_; 142 | throttle_pub_.publish(throttle_msg); 143 | 144 | // Utilize exponential average similar to OSCC's joystick commander for smoothing of joystick twitchy output 145 | steering_average_ = calc_exponential_average(steering_average_, steering_, DATA_SMOOTHING_FACTOR_); 146 | 147 | roscco::SteeringCommand steering_msg; 148 | steering_msg.header.stamp = ros::Time::now(); 149 | steering_msg.steering_torque = steering_average_; 150 | steering_pub_.publish(steering_msg); 151 | } 152 | } 153 | else 154 | { 155 | // Ensure the trigger values have been initialized 156 | if ((joy->axes[BRAKE_AXES_] > PARKED_THRESHOLD_) && (joy->axes[THROTTLE_AXES_] > PARKED_THRESHOLD_)) 157 | { 158 | initialized_ = true; 159 | } 160 | 161 | if (joy->axes[BRAKE_AXES_] <= PARKED_THRESHOLD_) 162 | { 163 | ROS_INFO("Pull the brake trigger to initialize."); 164 | } 165 | 166 | if (joy->axes[THROTTLE_AXES_] <= PARKED_THRESHOLD_) 167 | { 168 | ROS_INFO("Pull the throttle trigger to initilize."); 169 | } 170 | } 171 | } 172 | 173 | /** 174 | * @brief Calculate the exponential average 175 | * 176 | * Calculates and returns a new exponential moving average (EMA) based on the new values. 177 | * 178 | * @param AVERAGE The current average value. 179 | * @param SETPOINT The new datapoint to be included in the average. 180 | * @param FACTOR The coeffecient for smoothing rate, higher number yields faster discount of older values. 181 | * @return The new exponential average value the includes the new setpoint. 182 | */ 183 | double calc_exponential_average(const double AVERAGE, const double SETPOINT, const double FACTOR) 184 | { 185 | double exponential_average = (SETPOINT * FACTOR) + ((1.0 - FACTOR) * AVERAGE); 186 | 187 | return (exponential_average); 188 | } 189 | 190 | /** 191 | * @brief Remaps values from one linear range to another. 192 | * 193 | * Remap the value in an existing linear range to an new linear range example 0 in [-1, 1] to [0, 1] results in 0.5 194 | * 195 | * @param VALUE Data value to be remapped. 196 | * @param HIGH_1 High value of the old range 197 | * @param LOW_1 Low value of the old range 198 | * @param HIGH_2 High value of the new range 199 | * @param LOW_2 Low value of the new range 200 | * @return Data value mapped to the new range 201 | */ 202 | double linear_tranformation(const double VALUE, const double HIGH_1, const double LOW_1, const double HIGH_2, 203 | const double LOW_2) 204 | { 205 | return LOW_2 + (VALUE - LOW_1) * (HIGH_2 - LOW_2) / (HIGH_1 - LOW_1); 206 | } 207 | 208 | int main(int argc, char** argv) 209 | { 210 | ros::init(argc, argv, "roscco_teleop"); 211 | RosccoTeleop roscco_teleop; 212 | 213 | ros::spin(); 214 | } 215 | --------------------------------------------------------------------------------