diablo ready for test

.gitignore

@@ -0,0 +1,7 @@
+.catkin_workspace
+build/
+devel/
+src/CMakeLists.txt
+src/diablo-onboard-sdk/serial/build
+.vscode/
+*.pyc

README.md

@@ -0,0 +1,12 @@
+# diablo_mpc
+
+## a MPC for diablo configuration(see as differential car)
+
+if you want to use the code, you should install OSQP first, and can following steps:
+
+```
+cd test_mpc
+catkin_make
+source devel/setup.bash
+roslaunch mpc test_mpc.launch
+```

src/fake_ugv/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(kimatic_simulator)
+
+set(CMAKE_BUILD_TYPE "Release")
+#set(CMAKE_CXX_FLAGS "-std=c++11")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  nav_msgs
+  std_msgs
+  diablo_sdk
+)
+find_package(Eigen3 REQUIRED)
+
+catkin_package(
+ CATKIN_DEPENDS roscpp std_msgs diablo_sdk
+#  DEPENDS system_lib
+)
+
+catkin_package()
+
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
+)
+
+add_executable (kimatic_simulator_node src/poscmd_2_odom.cpp )
+target_link_libraries(kimatic_simulator_node
+  ${catkin_LIBRARIES})

src/fake_ugv/package.xml

@@ -0,0 +1,68 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>kimatic_simulator</name>
+  <version>0.0.0</version>
+  <description>a kimatic simulator package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">todo</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/map_generator</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>nav_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>nav_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <depend>diablo_sdk</depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/fake_ugv/src/poscmd_2_odom.cpp

@@ -0,0 +1,133 @@
+#include <iostream>
+#include <math.h>
+#include <random>
+#include <eigen3/Eigen/Dense>
+#include <ros/ros.h>
+#include <nav_msgs/Odometry.h>
+#include "diablo_sdk/Diablo_Ctrl.h"
+
+using namespace std;
+
+ros::Subscriber diablo_cmd_sub;
+ros::Publisher  odom_pub;
+ros::Timer simulate_timer;
+
+diablo_sdk::Diablo_Ctrl diablo_cmd;
+double x=0.0;
+double y=0.0;
+double speed = 0.0;
+double yaw = 0.0;
+double yaw_dot = 0.0;
+
+double p_init_x, p_init_y, p_init_z;
+double time_resolution = 0.01;
+double max_omega = 2.4;
+double max_speed = 1.5;
+
+bool rcv_cmd = false;
+
+void initParams()
+{
+	p_init_x = 0.0;
+	p_init_y = 0.0;
+	p_init_z = 0.0;
+}
+
+void rcvVelCmdCallBack(const diablo_sdk::Diablo_CtrlConstPtr cmd)
+{	
+	               rcv_cmd 	  = true;
+	diablo_cmd    = *cmd;
+    // ROS_INFO("in ODOM, the cmd is: a=%f, steer=%f", cmd.drive.acceleration, cmd.drive.steering_angle);
+}
+
+void normyaw(double& th)
+{
+	while (th > M_PI)
+		th -= M_PI * 2;
+	while (th < -M_PI)
+		th += M_PI * 2;
+}
+
+void simCallback(const ros::TimerEvent &e)
+{
+	nav_msgs::Odometry new_odom;
+
+	new_odom.header.stamp       = ros::Time::now();
+	new_odom.header.frame_id    = "world";
+
+	speed = diablo_cmd.speed;
+	yaw_dot = diablo_cmd.omega;
+	if (speed >= max_speed)
+	{
+		speed = max_speed;
+	}else if (speed<= -max_speed)
+	{
+		speed = -max_speed;
+	}
+	if (yaw_dot >= max_omega)
+	{
+		yaw_dot = max_omega;
+	}else if (yaw_dot<= -max_omega)
+	{
+		yaw_dot = -max_omega;
+	}
+	x = x + speed * cos(yaw) * time_resolution;
+	y = y + speed * sin(yaw) * time_resolution;
+	yaw = yaw + yaw_dot * time_resolution;
+
+	normyaw(yaw);    
+
+	new_odom.pose.pose.position.x  = x;
+	new_odom.pose.pose.position.y  = y;
+	new_odom.pose.pose.position.z  = 0;
+	new_odom.twist.twist.linear.x  = speed * cos(yaw);
+	new_odom.twist.twist.linear.y  = speed * sin(yaw);
+	new_odom.twist.twist.linear.z  = 0;
+	new_odom.twist.twist.angular.z = yaw_dot;	
+
+	Eigen::Vector3d xC(cos(yaw), sin(yaw), 0);
+	Eigen::Vector3d yC(-sin(yaw), cos(yaw), 0);
+	Eigen::Vector3d zC(0, 0, 1);
+	Eigen::Matrix3d R2;
+	R2.col(0) = xC;
+	R2.col(1) = yC;
+	R2.col(2) = zC;
+	Eigen::Quaterniond q2(R2);
+	new_odom.pose.pose.orientation.w = q2.w();
+	new_odom.pose.pose.orientation.x = q2.x();
+	new_odom.pose.pose.orientation.y = q2.y();
+	new_odom.pose.pose.orientation.z = q2.z();
+
+	odom_pub.publish(new_odom);
+
+}
+
+int main (int argc, char** argv) 
+{        
+    ros::init (argc, argv, "ugv_kinematic_model_node");
+    ros::NodeHandle nh( "~" );
+
+	initParams();
+	nh.getParam("simulator/max_omega", max_omega);
+	nh.getParam("simulator/max_speed", max_speed);
+
+    diablo_cmd_sub  = nh.subscribe( "command", 1000, rcvVelCmdCallBack );
+    odom_pub  = nh.advertise<nav_msgs::Odometry>("odometry", 10);
+
+	diablo_cmd.speed = 0.0;
+	diablo_cmd.omega = 0.0;
+
+    simulate_timer = nh.createTimer(ros::Duration(time_resolution), simCallback);
+	ros::spin();
+
+
+    // ros::Rate rate(100);
+
+    // while(ros::ok()) 
+    // {
+    //     ros::spinOnce();
+    //     rate.sleep();
+    // }
+
+    return 0;
+}

src/mpc/CMakeLists.txt

@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(mpc)
+
+set(CMAKE_BUILD_TYPE "Release")
+#set(CMAKE_CXX_FLAGS "-std=c++11")
+ADD_COMPILE_OPTIONS(-std=c++11 )
+ADD_COMPILE_OPTIONS(-std=c++14 )
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  nav_msgs
+  std_msgs
+  visualization_msgs
+  diablo_sdk
+)
+find_package(Eigen3 REQUIRED)
+# find_package(osqp REQUIRED)
+find_package(OsqpEigen REQUIRED)
+
+catkin_package(
+ CATKIN_DEPENDS roscpp std_msgs diablo_sdk
+#  DEPENDS system_lib
+)
+
+include_directories( 
+    SYSTEM 
+    include 
+    ${catkin_INCLUDE_DIRS}
+    ${Eigen3_INCLUDE_DIRS} 
+    ${PROJECT_SOURCE_DIR}/include
+)
+
+add_executable (mpc src/mpc.cpp src/cubic_spline_planner.cpp)
+target_link_libraries(mpc
+    # PRIVATE osqp::osqp
+    OsqpEigen::OsqpEigen
+    ${catkin_LIBRARIES}
+    )

src/mpc/include/cubic_spline_planner.h

@@ -0,0 +1,31 @@
+#pragma once
+#include <Eigen/Eigen>
+#include <Eigen/Dense>
+#include <Eigen/LU>
+#include <vector>
+#include <cmath>
+#include <numeric>
+
+using namespace std;
+using namespace Eigen;
+
+class cubic_spline_planner
+{
+private:
+	vector<double> x;
+	vector<double> y;
+	vector<double> t;
+	vector<VectorXd> coeff;
+	double ds;
+public:
+	cubic_spline_planner(const vector<double>& _x, const vector<double>& _y, const double& _ds);
+	vector<VectorXd> get_coeff(void);
+	Vector3d get_state(double time);
+	vector<Vector3d> get_path(void);
+	double get_duration(void)
+	{
+		return t.back();
+	}
+	~cubic_spline_planner() {}
+};
+