Skip to content

Commit

Permalink
first commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@maggielovedd
maggielovedd committed Jul 16, 2020
0 parents commit 99a6264
Show file tree
Hide file tree
Showing 484 changed files with 64,435 additions and 0 deletions.
40 changes: 40 additions & 0 deletions huanyu_robot_start/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,40 @@
cmake_minimum_required(VERSION 2.8.3)
project(huanyu_robot_start)

find_package(catkin REQUIRED COMPONENTS
actionlib
actionlib_msgs
geometry_msgs
message_generation
nav_msgs
nav_msgs
roscpp
serial
roslib
rospy
sensor_msgs
std_msgs
tf
)

# ulimit -c unlimited
# ssh [email protected]
add_compile_options(-g -std=c++11) #-g -> core dumped

catkin_package(
INCLUDE_DIRS include
LIBRARIES huanyu_robot_start
# CATKIN_DEPENDS geometry_msgs message_generation nav_msgs nav_msgs roscpp roslib rospy sensor_msgs std_msgs tf
DEPENDS system_lib
)

# defind robot status massage
# command atkin_make -DCATKIN_WHITELIST_PACKAGES="new_msg"
include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
include_directories(
include
include/${PROJECT_NAME}
)

add_executable(huanyu_robot_node src/Huanyu_robot.cpp)
target_link_libraries(huanyu_robot_node ${catkin_LIBRARIES})
7 changes: 7 additions & 0 deletions huanyu_robot_start/creat_huanyu_udev.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0002", MODE:="0777", GROUP:="dialout", SYMLINK+="huanyu_base"' >/etc/udev/rules.d/huanyu_base.rules
echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60",ATTRS{serial}=="0003", MODE:="0777", GROUP:="dialout", SYMLINK+="huanyu_laser"' >/etc/udev/rules.d/huanyu_laser.rules
service udev reload
sleep 2
service udev restart


164 changes: 164 additions & 0 deletions huanyu_robot_start/include/Huanyu_robot.h
Original file line number Diff line number Diff line change
@@ -0,0 +1,164 @@

#ifndef __HUANYU_ROBOT_H_
#define __HUANYU_ROBOT_H_
// sudo apt-get install ros-melodic-serial
// rostopic pub -r 10 /cmd_vel geometry_msgs/Twist '{linear: {x: 0.1, y: 0, z: 0}, angular: {x: 0, y: 0, z: -0.5}}'
// sudo usermod -aG　dialout wsh

#include "ros/ros.h"
#include <iostream>
#include <string.h>
#include <string>
#include <iostream>
#include <math.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <serial/serial.h>
#include <fcntl.h>
#include <stdbool.h>
#include <tf/transform_broadcaster.h>
#include <std_msgs/String.h>
#include <std_msgs/Float32.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Vector3.h>
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>


using namespace std;

#define RECIVER_DATA_HEADER 0XFEFEFEFE
#define RECIVER_DATA_CHECK_SUM 0XEE
#define PROTOBUF_SIZE 33

#define PI 3.1415926f
#define GYROSCOPE_RADIAN 0.001064f // gyro_x/(16.40*57.30)=gyro_x*0.001064 单位为弧度每秒
#define GYROSCOPE_DEGREE 16.40f // Accelerometer_Xdata/16.40=61度每秒
#define ACCELEROMETER 16384.0f // 1000/2048=0.49g。g为加速度的单位，重力加速度定义为1g, 等于9.8米每平方秒。

#define sampleFreq 20.0f // sample frequency in Hz
#define twoKpDef 1.0f // (2.0f * 0.5f) // 2 * proportional gain
#define twoKiDef 0.0f // (2.0f * 0.0f) // 2 * integral gain

#define OFFSET_COUNT 40

const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3};
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};

const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e3};
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0,
0, 1e-3, 1e-9, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-9};

#pragma pack(1)
typedef struct __Mpu6050_Str_
{
short X_data;
short Y_data;
short Z_data;
}Mpu6050_Str;

typedef union _Upload_Data_
{
unsigned char buffer[PROTOBUF_SIZE];

struct _Sensor_Str_
{
unsigned int Header;
float X_speed;
float Y_speed;
float Z_speed;

float Source_Voltage;

Mpu6050_Str Link_Accelerometer;
Mpu6050_Str Link_Gyroscope;

unsigned char End_flag;
}Sensor_Str;
}Upload_Data;
#pragma pack(4)


class Huanyu_start_object
{
public:
Huanyu_start_object();
~Huanyu_start_object();

/* /cmd_val topic call function */
void cmd_velCallback(const geometry_msgs::Twist &twist_aux);
void cmd_AmclvelCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &Pose);


/* Read/Write data from ttyUSB */
bool ReadFormUart();
bool WriteToUart(unsigned char*){}
bool ReadAndWriteLoopProcess();

/* This node Publisher topic and tf */
void PublisherOdom();
void publisherImuSensor();
void publisherImuSensorRaw();

float invSqrt(float number);
/*输入参数为加速度xyz原始数据，磁力计xyz原始数据，陀螺仪数据必须转换为弧度制*/
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az);
void accelerometerOffset(float gx, float gy, float gz);


serial::Serial Robot_Serial; //声明串口对象

private:
int baud_data;
string usart_port, robot_frame_id, smoother_cmd_vel;
float filter_Vx_match,filter_Vth_match;
bool imu_velocity_z;

/* Ros node define*/
ros::NodeHandle n;
ros::Time current_time, last_time;

ros::Subscriber cmd_vel_sub, amcl_sub;
ros::Publisher odom_pub, imu_pub, imu_pub_raw, power_pub;
tf::TransformBroadcaster odom_broadcaster;

Upload_Data Reciver_Str, Send_Str;

/* Odom and tf value*/
double x, y, th, vx, vy, vth, dt;

sensor_msgs::Imu Mpu6050;
float Gyroscope_Xdata_Offset;
float Gyroscope_Ydata_Offset;
float Gyroscope_Zdata_Offset;
float Power_valtage;

unsigned short Offest_Count;
};


#endif


73 changes: 73 additions & 0 deletions huanyu_robot_start/launch/Huanyu_robot_start.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,73 @@
<launch>

<node pkg="tf" type="static_transform_publisher" name="base_to_link" args="0 0 0 0 0 0 base_footprint base_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_gyro" args="0 0 0 0 0 0 base_footprint gyro_link 100" />
<node pkg="tf" type="static_transform_publisher" name="base_to_laser" args="0.15 0 0.05 3.14159 0 0 base_footprint laser 100" />

<param name = "robot_description" textfile = "$(find huanyubot_description)/urdf/huanyubot.urdf"/>

<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

<!--Start the robot's base control code and publish the odometer data-->

<node pkg="huanyu_robot_start" type="huanyu_robot_node" name="publish_odom" output="screen">
<param name="usart_port" type="string" value="/dev/huanyu_base"/>
<param name="baud_data" type="int" value="115200"/>
<param name="robot_frame_id" type="string" value="base_footprint"/>
<param name="smoother_cmd_vel" type="string" value="smoother_cmd_vel"/>
<param name="imu_velocity_z" type="bool" value="false"/>
<param name="filter_Vx_match" type="double" value="1.0"/>
<param name="filter_Vth_match" type="double" value="1.0"/>
</node>

<!--node pkg="hls_lfcd_lds_driver" type="hlds_laser_publisher" name="hlds_laser_publisher" output="screen">
<param name="port" value="/dev/huanyu_laser"/>
<param name="baud_rate" value="230400"/>
<param name="frame_id" value="laser"/>
</node-->

<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
<param name="serial_port" type="string" value="/dev/huanyu_laser"/>
<param name="serial_baudrate" type="int" value="115200"/>
<param name="frame_id" type="string" value="laser"/>
<param name="inverted" type="bool" value="false"/>
<param name="angle_compensate" type="bool" value="true"/>
</node>


<!-- Robot pose ekf -->
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="1.0"/>
<param name="odom_used" value="true"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
<remap from="/imu_data" to="/mobile_base/sensors/imu_data" />
</node>


<arg name="node_name" value="velocity_smoother"/>
<arg name="nodelet_manager_name" value="nodelet_manager"/>
<arg name="config_file" value="$(find huanyu_robot_start)/param/Huanyu_smoother.yaml"/>
<arg name="raw_cmd_vel_topic" value="cmd_vel"/>
<arg name="smooth_cmd_vel_topic" value="smoother_cmd_vel"/>
<arg name="robot_cmd_vel_topic" value="robot_cmd_vel"/>
<arg name="odom_topic" value="odom_combined"/>


<node pkg="nodelet" type="nodelet" name="$(arg nodelet_manager_name)" args="manager"/>

<include file="$(find yocs_velocity_smoother)/launch/velocity_smoother.launch">
<arg name="node_name" value="$(arg node_name)"/>
<arg name="nodelet_manager_name" value="$(arg nodelet_manager_name)"/>
<arg name="config_file" value="$(arg config_file)"/>
<arg name="raw_cmd_vel_topic" value="$(arg raw_cmd_vel_topic)"/>
<arg name="smooth_cmd_vel_topic" value="$(arg smooth_cmd_vel_topic)"/>
<arg name="robot_cmd_vel_topic" value="$(arg robot_cmd_vel_topic)"/>
<arg name="odom_topic" value="$(arg odom_topic)"/>
</include>

</launch>


47 changes: 47 additions & 0 deletions huanyu_robot_start/launch/amcl_test.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,47 @@
<launch>

<arg name="map_file" default="$(find huanyu_robot_start)/map/map.yaml"/>
<node name="map_server_for_test" pkg="map_server" type="map_server" args="$(arg map_file)">
</node>

<arg name="use_map_topic" default="false"/>
<arg name="scan_topic" default="scan"/>

<node pkg="amcl" type="amcl" name="amcl" clear_params="true">
<param name="use_map_topic" value="$(arg use_map_topic)"/>
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="60"/>
<param name="laser_max_range" value="12.0"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="2000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.2"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.25"/>
<param name="update_min_a" value="0.2"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<!-- Increase tolerance because the computer can get quite busy -->
<param name="transform_tolerance" value="1.0"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>

</launch>
45 changes: 45 additions & 0 deletions huanyu_robot_start/launch/gmapping_slam.launch
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@


<launch>
<arg name="scan_topic" default="scan" />
<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="odom_combined"/>

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
<param name="base_frame" value="$(arg base_frame)"/>
<param name="odom_frame" value="$(arg odom_frame)"/>
<param name="map_update_interval" value="0.01"/>
<param name="maxUrange" value="4.0"/>
<param name="maxRange" value="5.0"/>
<param name="sigma" value="0.05"/>
<param name="kernelSize" value="3"/>
<param name="lstep" value="0.05"/>
<param name="astep" value="0.05"/>
<param name="iterations" value="5"/>
<param name="lsigma" value="0.075"/>
<param name="ogain" value="3.0"/>
<param name="lskip" value="0"/>
<param name="minimumScore" value="30"/>
<param name="srr" value="0.01"/>
<param name="srt" value="0.02"/>
<param name="str" value="0.01"/>
<param name="stt" value="0.02"/> -->
<param name="linearUpdate" value="0.05"/>
<param name="angularUpdate" value="0.0436"/>
<param name="temporalUpdate" value="-1.0"/>
<param name="resampleThreshold" value="0.5"/>
<param name="particles" value="8"/>

<param name="xmin" value="-1.0"/>
<param name="ymin" value="-1.0"/>
<param name="xmax" value="1.0"/>
<param name="ymax" value="1.0"/>

<param name="delta" value="0.05"/>
<param name="llsamplerange" value="0.01"/>
<param name="llsamplestep" value="0.01"/>
<param name="lasamplerange" value="0.005"/>
<param name="lasamplestep" value="0.005"/>
<remap from="scan" to="$(arg scan_topic)"/>
</node>
</launch>
Loading

0 comments on commit 99a6264

Please sign in to comment.