1./home/hao/catkin_lh/src/arbotix_ros
arbotix_ros是官方给的包 sudo apt-get install ros-kinetic-arbotix-*
具体的下载位置ros工程包的scr中
作用：给出了仿真机器人需要的里程计和TF坐标转化关系
launch文件调用代码
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" clear_params="true">
<rosparam file="$(find car_bringup)/config/fake_arbotix.yaml" command="load" />
<param name="sim" value="false"/>
</node>
config文件内容：
port: /dev/ttyUSB0
baud: 115200
rate: 20
sync_write: True
sync_read: True
read_rate: 20
write_rate: 20
controllers: {
# Pololu motors: 1856 cpr = 0.3888105m travel = 4773 ticks per meter (empirical: 4100) 这里的读数跟编码器有关
base_controller: {type: diff_controller, base_frame_id: base_footprint, base_width: 0.9, ticks_meter: 4100, Kp: 12, Kd: 12, Ki: 0, Ko: 50, accel_limit: 1.0 }
}
这些都是虚拟的，正是因为定义了轮子尺寸，每秒走的脉冲数，pid参数，才得到了最后的机器人的轮式里程计。
注意的是如果想要实现现实机器人的导航一定要将这个代码屏蔽掉，否则这部分发布一个假的里程即会影响真实的里程计会产生冲突。
2./home/hao/catkin_lh/src/car_bringup
fake_car.launch
<launch>
<param name="/use_sim_time" value="false" />
<!-- Load the URDF/Xacro model of our robot
加入机器人的模型
运行arbotix_python仿真所需节点
发布机器人状态信息
-->
<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find car_description)/urdf/turtlebot.urdf.xacro'" />
<param name="robot_description" command="$(arg urdf_file)" />
<!--
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen" clear_params="true">
<rosparam file="$(find car_bringup)/config/fake_arbotix.yaml" command="load" />
<param name="sim" value="false"/>
</node>
-->
<!--tf -->
<node name="link_laser" pkg="tf" type="static_transform_publisher" args="0.85 0 0.33 0 0 0 base_link laser 100"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
<param name="publish_frequency" type="double" value="20.0" />
</node>
</launch>
这里的launch文件屏蔽掉了仿真导航的机器人的里程TF关系，加入了真实机器人的机器人TF
坐标转换关系，这里表明激光雷达安装在车体中心的x轴85cm z轴方向0.33cm，没有旋转。
3./home/hao/catkin_lh/src/car_description
这部分完全是仿真机器人的模型的编写
从solidworks导出urdf，重要的信息是要这定机器人的中心坐标系，也就是旋转坐标系，
然后把地面移动平台内部的零部件删除掉，否则会产生卡死的情况，再分级部分，比如地面
移动平台为两个激光，四个轮子和车身，都出要写好谁是谁的父子链接关系，标明每个link
和joint的名称。
主要对一下两个文件进行了修改：
当然除此之外还要写好stl文件的路径，这样在rviz中才能正常显示.
turtlebot.urdf.xacro
<?xml version="1.0"?>
<robot name="turtlebot"
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
xmlns:xacro="http://ros.org/wiki/xacro">
<!-- <xacro:include filename="$(find rbx1_description)/urdf/turtlebot_hardware.urdf.xacro" /> -->
<xacro:include filename="$(find car_description)/urdf/turtlebot_body.urdf.xacro" />
<!--<xacro:include filename="$(find rbx1_description)/urdf/turtlebot_calibration.xacro" />
<xacro:include filename="$(find rbx1_description)/urdf/turtlebot_kinect.urdf.xacro" /> -->
<property name="M_PI" value="3.14159"/>
<property name="SCALE" value="0.0254"/>
<!-- Body of TurtleBot, with plates, standoffs and Create (including sim sensors) -->
<turtlebot_body/>
<!-- Kinect frames and mounting (includes simulation)
<turtlebot_kinect/> -->
</robot>
turtlebot_body.urdf.xacro
<?xml version="1.0"?>
<robot name="turtlebot_body" xmlns:xacro="http://ros.org/wiki/xacro">
<property name="M_PI" value="3.14159"/>
<property name="MESH_SCALE" value="100"/>
<!-- Macro for TurtleBot body. Including Gazebo extensions, but does not include Kinect -->
<xacro:include filename="$(find car_description)/urdf/gazebo.urdf.xacro"/>
<xacro:macro name="turtlebot_body">
<material name="Green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin, navigation stack depends on this frame -->
<link name="base_footprint">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
<material name="Green" />
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<box size="0.001 0.001 0.001" />
</geometry>
</collision>
</link>
<link name="base_link">
<inertial>
<mass value="21.9204478254844" />
<origin xyz="0 0 0.0" />
<inertia ixx="0.916631161416712" ixy="-4.30628165148592E-05" ixz="0.0407461026020485"
iyy="2.32223261991067" iyz="-0.000882178736936557" izz="3.11137256533784" />
</inertial>
<visual>
<origin xyz=" 0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/base_link.STL" />
</geometry>
<material
name="">
<color
rgba="0.8529 0.8529 0.8529 1" />
</material>
</visual>
<collision>
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/base_link.STL"/>
</geometry>
</collision>
</link>
<joint name="base_footprint_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0" />
<parent link="base_footprint"/>
<child link="base_link" />
</joint>
<!-- base link -->
<link name="fl_wheel_link">
<inertial>
<origin xyz="0.62049757316184 0.40477862698146 -0.00251993245342085"/>
<mass value="1.23582190055061" />
<inertia
ixx="0.00326997144053353"
ixy="-6.45551206208396E-12"
ixz="4.59997144251533E-11"
iyy="0.00613329122337224"
iyz="-1.77702992056636E-10"
izz="0.00326997154694097" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/fl_wheel_link.STL"/>
</geometry>
<material
name="">
<color
rgba="0.4117647059 0.4117647059 0.4117647059 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh
filename="package://car_description/meshes/fl_wheel_link.STL" />
</geometry>
</collision>
</link>
<joint name="base_fl_wheel_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="fl_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<!-- front wheel -->
<link name="fr_wheel_link">
<inertial>
<origin xyz="0.620493859404575 -0.404837547831419 -0.00252043211921564"/>
<mass value="1.2378164091277" />
<inertia
ixx="0.00327138338683444"
ixy="1.16223985940428E-07"
ixz="2.60795908304402E-11"
iyy="0.00613348560468621"
iyz="3.16100072328905E-11"
izz="0.00327133842694576" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/fr_wheel_link.STL"/>
</geometry>
<material
name="">
<color
rgba="0.4117647059 0.4117647059 0.4117647059 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/fr_wheel_link.STL"/>
</geometry>
</collision>
</link>
<joint name="base_fr_wheel_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="fr_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<!--back__left_wheel-->
<link name="bl_wheel_link">
<inertial>
<origin xyz="-0.0014987 0.40484 -0.0025204"/>
<mass value="1.2378" />
<inertia
ixx="0.0032714"
ixy="1.1624E-07"
ixz="-1.6758E-11"
iyy="0.0061335"
iyz="-1.9554E-11"
izz="0.0032713" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/bl_wheel_link.STL"/>
</geometry>
<material
name="">
<color
rgba="0.4117647059 0.4117647059 0.4117647059 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh
filename="package://car_description/meshes/bl_wheel_link.STL" />
</geometry>
</collision>
</link>
<joint name="base_bl_wheel_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="bl_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<!-- back_right_wheel -->
<link name="br_wheel_link">
<inertial>
<origin xyz="-0.0014987 -0.40484 -0.0025204"/>
<mass value="1.2378164091277" />
<inertia
ixx="0.00327138338683444"
ixy="1.16223985940428E-07"
ixz="2.60795908304402E-11"
iyy="0.00613348560468621"
iyz="3.16100072328905E-11"
izz="0.00327133842694576" />
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/br_wheel_link.STL"/>
</geometry>
<material
name="1.2378">
<color
rgba="0.4117647059 0.4117647059 0.4117647059 1" />
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://car_description/meshes/br_wheel_link.STL"/>
</geometry>
</collision>
</link>
<joint name="base_br_wheel_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="br_wheel_link"/>
<axis xyz="0 1 0"/>
</joint>
<!--laser -->
<link name="laser">
<inertial>
<origin
xyz="0.841589474602505 7.73215478295855E-05 0.147497262797329"
rpy="0 0 0" />
<mass value="1.24741093230714" />
<inertia ixx="0.00314639570294894"
ixy="5.51048311402194E-06"
ixz="-0.000200686547775832"
iyy="0.00322539936792015"
iyz="4.60077868668151E-06"
izz="0.00207394604621157" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://car_description/meshes/hokuyo.dae" />
</geometry>
<material
name="">
<color
rgba="1 0.5490196078 0 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://car_description/meshes/hokuyo.dae" />
</geometry>
</collision>
</link>
<joint name="laser_joint" type="fixed">
<origin
xyz="0 0 0"
rpy="0 0 0" />
<parent
link="base_link" />
<child
link="laser" />
<axis
xyz="0 0 0" />
</joint>
<!-- Simulation sensors
<turtlebot_sim_imu/>
<turtlebot_sim_laser/>
<turtlebot_sim_create/>
<turtlebot_sim_wall_sensors/> -->
</xacro:macro>
</robot>
4. /home/hao/catkin_lh/src/car_msgs
这里自定义了carspeed消息格式，主要用于传送在底层驱动程序
中通过轮子脉冲算出的里程计所需要的vx vy vth数据
还有nav_100的自定义消息，主要用于传送组合导航的所有数据。
注意：如果订阅和发布的程序卸载里一个ros包中，一定要记得在
另一个包中配置自定义消息的工作，否则会产生缺少.h文件的错误.
5./home/hao/catkin_lh/src/car_nav
/home/hao//catkin_lh/src/car_nav/launch/amcl_diff.launch
/home/hao//catkin_lh/src/car_nav/launch/car_amcl.launch
/home/hao//catkin_lh/src/car_nav/launch/fake_amcl.launch
/home/hao//catkin_lh/src/car_nav/launch/fake_move_base_amcl.launch
这部分配置move—base中需要的参数文件
launch文件如此：
<launch>
<param name="use_sim_time" value="false" />
<!-- 导入地图文件 -->
<arg name="map" default="test58.yaml" />
<!-- 运行希望的地图服务器 -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find car_nav)/maps/$(arg map)"/>
<!-- The move_base node -->
<include file="$(find car_nav)/launch/fake_move_base_amcl.launch" />
<!-- 运行自适应蒙特卡洛定位的仿真节点
<node pkg="fake_localization" type="fake_localization" name="fake_localization" clear_params="true" output="screen">
<remap from="base_pose_ground_truth" to="odom" />
<param name="global_frame_id" value="map" />
<param name="base_frame_id" value="base_footprint" />
</node> -->
<!-- 真实的amcl -->
<include file="$(find car_nav)/launch/car_amcl.launch" />
</launch>
首先加载地图数据，在加载movebase文件
costmap_common_params.yaml costmap_common_params.yaml
local_costmap_params.yaml global_costmap_params.yaml
base_local_planner_params.yaml
再开启自适应蒙特卡罗定位。
6./home/hao/catkin_lh/src/cartographer_ros
cartographer的ROS接口
7./home/hao/catkin_lh/src/lms1xx
雷达启动程序，详见lms雷达启动另一篇博客
8./home/hao/catkin_lh/src/movecar
这部分写了整个车子的核心文件
movecar.launch
<launch>
<!-- 底盘控制（键盘或者是movebase发布的cmd_vel）-->
<node pkg="movecar"
type="base_controller"
name="base_controller"
output="screen">
</node>
<!-- 雷达 -->
<arg name="host" default="192.168.0.1" />
<arg name="publish_min_range_as_inf" default="false" />
<node pkg="lms1xx" name="lms1xx" type="LMS1xx_node">
<param name="host" value="$(arg host)" />
<param name="publish_min_range_as_inf" value="$(arg publish_min_range_as_inf)" />
</node>
<!-- 里程计 -->
<node pkg="movecar"
type="odometry_publisher_car"
name="odometry_publisher_car"
output="screen">
</node>
<!-- 罗技节点（主要用来防止意外控制启停） -->
<node respawn="true" pkg="joy" type="joy_node" name="joystick" output="screen">
<param name="dev" value="/dev/input/js0" type="string"/>
<param name="deadzone" value="0.12" />
</node>
</launch>
/******************************************************************
订阅了/cmd_vel主题，只要向该主题发布消息，就能实现对控制小车的移动
再正确配置串口就可以了 发布了dx dy dth
*******************************************************************/
#include "ros/ros.h" //ros需要的头文件
#include <geometry_msgs/Twist.h>
#include "std_msgs/String.h"
#include <car_msgs/carspeed.h>
#include <sensor_msgs/Joy.h>
//以下为串口通讯需要的头文件
//#include <string>
//#include <iostream>
#include <cstdio>
//#include <unistd.h>
//#include <math.h>
//#include "serial/serial.h"
/****************************************************************************/
#include <iostream>
#include "Definitions.h"
#include <string.h>
#include <sstream>
#include <unistd.h>
#include <getopt.h>
#include <stdlib.h>
#include <stdio.h>
#include <list>
#include <math.h>
#include <sys/types.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/times.h>
#include <sys/time.h>
typedef void* HANDLE;
typedef int BOOL;
using namespace std;
void* g_pKeyHandle = 0;
unsigned short g_usNodeId = 1;
string g_deviceName;
string g_protocolStackName;
string g_interfaceName;
string g_portName;
int g_baudrate = 0;
const string g_programName = "EPOS_MAIN";
void* g_pKeyHandle2 = 0;
unsigned short g_usNodeId2 = 1;
string g_deviceName2;
string g_protocolStackName2;
string g_interfaceName2;
string g_portName2;
int g_baudrate2 = 0;
const string g_programName2 = "EPOS_MAIN";
//--------------------提取速度 脉冲 时间信息//
int actual_v_left;
int actual_v_right;
int actual_p_left;
int actual_p_right;
// int actual_p_left1;
// int actual_p_right1;
int last_actual_p_left=0;
int last_actual_p_right=0;
// int last_actual_p_left1=0;
// int last_actual_p_right1=0;
int diff_actual_p_left=0;
int diff_actual_p_right=0;
struct timeval timenow_value;
long lasttime=0;
long lasttime1=0;
//-------------------------------
//--------------------------------手柄
void logcallback(const sensor_msgs::Joy::ConstPtr& Joy);
float axis_ang1,axis_lin1,axis_ang2,axis_lin2,axis_ang3,axis_lin3,ltadd1,rtadd1;
int ton[11];//axes[]的键 button[]
#ifndef MMC_SUCCESS
#define MMC_SUCCESS 0
#endif
#ifndef MMC_FAILED
#define MMC_FAILED 1
#endif
#ifndef MMC_MAX_LOG_MSG_SIZE
#define MMC_MAX_LOG_MSG_SIZE 512
#endif
void LogError(string functionName, int p_lResult, unsigned int p_ulErrorCode);
void LogInfo(string message);
void PrintUsage();
void PrintHeader();
void PrintSettings();
int OpenDevice(unsigned int* p_pErrorCode);
int CloseDevice(unsigned int* p_pErrorCode);
void SetDefaultParameters();
int ParseArguments(int argc, char** argv);
int DemoProfilePositionMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode);
int Demo(unsigned int* p_pErrorCode);
void PrintUsage()
{
cout << "Usage: HelloEposCmd -h -n 1 -d deviceName -s protocolStackName -i interfaceName -p portName -b baudrate" << endl;
cout << "Usage: HelloEposCmd -h -n 1 -d deviceName -s protocolStackName -i interfaceName -p portName -b baudrate" << endl;
}
void LogError(string functionName, int p_lResult, unsigned int p_ulErrorCode)
{
cerr << g_programName << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< endl;
cerr << g_programName2 << ": " << functionName << " failed (result=" << p_lResult << ", errorCode=0x" << std::hex << p_ulErrorCode << ")"<< endl;
}
void LogInfo(string message)
{
cout << message << endl;
}
void SeparatorLine()
{
const int lineLength = 60;
for(int i=0; i<lineLength; i++)
{
cout << "-";
}
cout << endl;
}
void PrintSettings()
{
stringstream msg;
msg << "default settings:" << endl;
msg << "node id = " << g_usNodeId << endl;
msg << "device name = '" << g_deviceName << "'" << endl;
msg << "protocal stack name = '" << g_protocolStackName << "'" << endl;
msg << "interface name = '" << g_interfaceName << "'" << endl;
msg << "port name = '" << g_portName << "'"<< endl;
msg << "baudrate = " << g_baudrate;
LogInfo(msg.str());
SeparatorLine();
// -------------------------------------------------------
stringstream msg2;
msg2 << "default settings:" << endl;
msg2 << "node id = " << g_usNodeId2 << endl;
msg2 << "device name = '" << g_deviceName2 << "'" << endl;
msg2 << "protocal stack name = '" << g_protocolStackName2 << "'" << endl;
msg2 << "interface name = '" << g_interfaceName2 << "'" << endl;
msg2 << "port name = '" << g_portName2 << "'"<< endl;
msg2 << "baudrate = " << g_baudrate2;
LogInfo(msg2.str());
SeparatorLine();
}
void SetDefaultParameters()
{
//USB
g_usNodeId =1;
g_deviceName = "EPOS"; //EPOS version
g_protocolStackName = "MAXON_RS232"; //MAXON_RS232 MAXON SERIAL V2
g_interfaceName = "RS232"; //RS232 USB
g_portName = "/dev/ttyS100"; // /dev/ttyS1 USB0
g_baudrate = 38400; //115200
//----------------------------------------------------
g_usNodeId2 =2;
g_deviceName2 = "EPOS"; //EPOS version
g_protocolStackName2 = "MAXON_RS232"; //MAXON_RS232 MAXON SERIAL V2
g_interfaceName2 = "RS232"; //RS232 USB
g_portName2 = "/dev/ttyS101"; // /dev/ttyS1 USB0
g_baudrate2 = 38400; //115200
}
int OpenDevice(unsigned int* p_pErrorCode)
{
int lResult = MMC_FAILED;
char* pDeviceName = new char[255];
char* pProtocolStackName = new char[255];
char* pInterfaceName = new char[255];
char* pPortName = new char[255];
strcpy(pDeviceName, g_deviceName.c_str());
strcpy(pProtocolStackName, g_protocolStackName.c_str());
strcpy(pInterfaceName, g_interfaceName.c_str());
strcpy(pPortName, g_portName.c_str());
LogInfo("Open device...");
g_pKeyHandle = VCS_OpenDevice(pDeviceName, pProtocolStackName, pInterfaceName, pPortName, p_pErrorCode);
if(g_pKeyHandle!=0 && *p_pErrorCode == 0)
{
unsigned int lBaudrate = 0;
unsigned int lTimeout = 0;
if(VCS_GetProtocolStackSettings(g_pKeyHandle, &lBaudrate, &lTimeout, p_pErrorCode)!=0)
{
if(VCS_SetProtocolStackSettings(g_pKeyHandle, g_baudrate, lTimeout, p_pErrorCode)!=0)
{
if(VCS_GetProtocolStackSettings(g_pKeyHandle, &lBaudrate, &lTimeout, p_pErrorCode)!=0)
{
if(g_baudrate==(int)lBaudrate)
{
lResult = MMC_SUCCESS;
}
}
}
}
}
else
{
g_pKeyHandle = 0;
}
delete []pDeviceName;
delete []pProtocolStackName;
delete []pInterfaceName;
delete []pPortName;
//------------------------------------------------------------------------------------------------
char* pDeviceName2 = new char[255];
char* pProtocolStackName2 = new char[255];
char* pInterfaceName2 = new char[255];
char* pPortName2 = new char[255];
strcpy(pDeviceName2, g_deviceName2.c_str());
strcpy(pProtocolStackName2, g_protocolStackName2.c_str());
strcpy(pInterfaceName2, g_interfaceName2.c_str());
strcpy(pPortName2, g_portName2.c_str());
LogInfo("Open device...");
g_pKeyHandle2 = VCS_OpenDevice(pDeviceName2, pProtocolStackName2, pInterfaceName2, pPortName2, p_pErrorCode);
if(g_pKeyHandle2!=0 && *p_pErrorCode == 0)
{
unsigned int lBaudrate = 0;
unsigned int lTimeout = 0;
if(VCS_GetProtocolStackSettings(g_pKeyHandle2, &lBaudrate, &lTimeout, p_pErrorCode)!=0)
{
if(VCS_SetProtocolStackSettings(g_pKeyHandle2, g_baudrate2, lTimeout, p_pErrorCode)!=0)
{
if(VCS_GetProtocolStackSettings(g_pKeyHandle2, &lBaudrate, &lTimeout, p_pErrorCode)!=0)
{
if(g_baudrate2==(int)lBaudrate)
{
lResult = MMC_SUCCESS;
}
}
}
}
}
else
{
g_pKeyHandle2 = 0;
}
delete []pDeviceName2;
delete []pProtocolStackName2;
delete []pInterfaceName2;
delete []pPortName2;
return lResult;
}
int CloseDevice(unsigned int* p_pErrorCode)
{
int lResult = MMC_FAILED;
*p_pErrorCode = 0;
LogInfo("Close device");
if(VCS_CloseDevice(g_pKeyHandle, p_pErrorCode)!=0 && *p_pErrorCode == 0)
{
lResult = MMC_SUCCESS;
}
//--------------------------------------------------------
// int lResult = MMC_FAILED;
*p_pErrorCode = 0;
LogInfo("Close device");
if(VCS_CloseDevice(g_pKeyHandle2, p_pErrorCode)!=0 && *p_pErrorCode == 0)
{
lResult = MMC_SUCCESS;
}
return lResult;
}
int ParseArguments(int argc, char** argv)
{
int lOption;
int lResult = MMC_SUCCESS;
// Shut GetOpt error messages down (return '?'):
opterr = 0;
// Retrieve the options:
while ( (lOption = getopt(argc, argv, ":hd:s:i:p:b:n:")) != -1 )
{
switch ( lOption ) {
case 'h':
PrintUsage(); //Usage: HelloEposCmd -h -n 1 -d deviceName
//-s protocolStackName -i interfaceName -p portName -b baudrate
lResult = 1;
break;
case 'd':
g_deviceName = optarg; //this is a special
break;
case 's':
g_protocolStackName = optarg;
break;
case 'i':
g_interfaceName = optarg;
break;
case 'p':
g_portName = optarg;
break;
case 'b':
g_baudrate = atoi(optarg);
break;
case 'n':
g_usNodeId = (unsigned short)atoi(optarg);
break;
case '?': // unknown option...
stringstream msg;
msg << "Unknown option: '" << char(optopt) << "'!";
LogInfo(msg.str());
PrintUsage();
lResult = MMC_FAILED;
break;
}
}
//-----------------------------------------------------------------
// int lOption;
lResult = MMC_SUCCESS;
// Shut GetOpt error messages down (return '?'):
opterr = 0;
// Retrieve the options:
while ( (lOption = getopt(argc, argv, ":hd:s:i:p:b:n:")) != -1 )
{
switch ( lOption ) {
case 'h':
PrintUsage(); //Usage: HelloEposCmd -h -n 1 -d deviceName
//-s protocolStackName -i interfaceName -p portName -b baudrate
lResult = 1;
break;
case 'd':
g_deviceName2 = optarg; //this is a special
break;
case 's':
g_protocolStackName2 = optarg;
break;
case 'i':
g_interfaceName2 = optarg;
break;
case 'p':
g_portName2 = optarg;
break;
case 'b':
g_baudrate2 = atoi(optarg);
break;
case 'n':
g_usNodeId2 = (unsigned short)atoi(optarg);
break;
case '?': // unknown option...
stringstream msg;
msg << "Unknown option: '" << char(optopt) << "'!";
LogInfo(msg.str());
PrintUsage();
lResult = MMC_FAILED;
break;
}
}
return lResult;
}
int DemoProfilePositionMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode)
{
int lResult = MMC_SUCCESS;
stringstream msg;
msg << "set profile position mode, node = " << p_usNodeId;
LogInfo(msg.str());
if(VCS_ActivateProfilePositionMode(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0)
{
LogError("VCS_ActivateProfilePositionMode", lResult, p_rlErrorCode);
lResult = MMC_FAILED;
}
else
{
list<long> positionList;
positionList.push_back(5000);
positionList.push_back(-20000);
positionList.push_back(20000);
positionList.push_back(-10000);
positionList.push_back(10000);
positionList.push_back(-10000);
positionList.push_back(5000);
for(list<long>::iterator it = positionList.begin(); it !=positionList.end(); it++)
{
long targetPosition = (*it);
stringstream msg;
msg << "move to position = " << targetPosition << ", node = " << p_usNodeId;
LogInfo(msg.str());
if(VCS_MoveToPosition(p_DeviceHandle, p_usNodeId, targetPosition, 0, 1, &p_rlErrorCode) == 0)
{
LogError("VCS_MoveToPosition", lResult, p_rlErrorCode);
lResult = MMC_FAILED;
break;
}
sleep(1);
}
if(lResult == MMC_SUCCESS)
{
LogInfo("halt position movement");
if(VCS_HaltPositionMovement(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0)
{
LogError("VCS_HaltPositionMovement", lResult, p_rlErrorCode);
lResult = MMC_FAILED;
}
}
}
return lResult;
}
bool DemoProfileVelocityMode(HANDLE p_DeviceHandle, unsigned short p_usNodeId, unsigned int & p_rlErrorCode)
{
int lResult = MMC_SUCCESS;
stringstream msg;
msg << "set profile velocity mode, node = " << p_usNodeId;
LogInfo(msg.str());
if(VCS_ActivateProfileVelocityMode(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0)
{
LogError("VCS_ActivateProfileVelocityMode", lResult, p_rlErrorCode);
lResult = MMC_FAILED;
}
else
{
list<long> velocityList;
velocityList.push_back(500);
velocityList.push_back(300);
velocityList.push_back(600);
velocityList.push_back(900);
velocityList.push_back(200);
for(list<long>::iterator it = velocityList.begin(); it !=velocityList.end(); it++)
{
long targetvelocity = (*it); //mubiaosudu
stringstream msg;
msg << "move with target velocity = " << targetvelocity << " rpm, node = " << p_usNodeId;
LogInfo(msg.str());
if(VCS_MoveWithVelocity(p_DeviceHandle, p_usNodeId, targetvelocity, &p_rlErrorCode) == 0)
{
lResult = MMC_FAILED;
LogError("VCS_MoveWithVelocity", lResult, p_rlErrorCode);
break;
}
sleep(3);
}
if(lResult == MMC_SUCCESS)
{
LogInfo("halt velocity movement");
if(VCS_HaltVelocityMovement(p_DeviceHandle, p_usNodeId, &p_rlErrorCode) == 0)
{
lResult = MMC_FAILED;
LogError("VCS_HaltVelocityMovement", lResult, p_rlErrorCode);
}
}
}
return lResult;
}
int PrepareDemo(unsigned int* p_pErrorCode)
{
int lResult = MMC_SUCCESS;
BOOL oIsFault = 0;
if(VCS_GetFaultState(g_pKeyHandle, g_usNodeId, &oIsFault, p_pErrorCode ) == 0)
{
LogError("VCS_GetFaultState", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
if(lResult==0)
{
if(oIsFault)
{
stringstream msg;
msg << "clear fault, node = '" << g_usNodeId << "'";
LogInfo(msg.str());
if(VCS_ClearFault(g_pKeyHandle, g_usNodeId, p_pErrorCode) == 0)
{
LogError("VCS_ClearFault", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
}
if(lResult==0)
{
BOOL oIsEnabled = 0;
if(VCS_GetEnableState(g_pKeyHandle, g_usNodeId, &oIsEnabled, p_pErrorCode) == 0)
{
LogError("VCS_GetEnableState", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
if(lResult==0)
{
if(!oIsEnabled)
{
if(VCS_SetEnableState(g_pKeyHandle, g_usNodeId, p_pErrorCode) == 0)
{
LogError("VCS_SetEnableState", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
}
}
}
}
//-----------------------------------------------------------------------------------------
lResult = MMC_SUCCESS;
oIsFault = 0;
if(VCS_GetFaultState(g_pKeyHandle2, g_usNodeId2, &oIsFault, p_pErrorCode ) == 0)
{
LogError("VCS_GetFaultState", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
if(lResult==0)
{
if(oIsFault)
{
stringstream msg;
msg << "clear fault, node = '" << g_usNodeId2 << "'";
LogInfo(msg.str());
if(VCS_ClearFault(g_pKeyHandle2, g_usNodeId2, p_pErrorCode) == 0)
{
LogError("VCS_ClearFault", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
}
if(lResult==0)
{
BOOL oIsEnabled = 0;
if(VCS_GetEnableState(g_pKeyHandle2, g_usNodeId2, &oIsEnabled, p_pErrorCode) == 0)
{
LogError("VCS_GetEnableState", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
if(lResult==0)
{
if(!oIsEnabled)
{
if(VCS_SetEnableState(g_pKeyHandle2, g_usNodeId2, p_pErrorCode) == 0)
{
LogError("VCS_SetEnableState", lResult, *p_pErrorCode);
lResult = MMC_FAILED;
}
}
}
}
}
return lResult;
}
int Demo(unsigned int* p_pErrorCode)
{
int lResult = MMC_SUCCESS;
unsigned int lErrorCode = 0;
lResult = DemoProfileVelocityMode(g_pKeyHandle, g_usNodeId, lErrorCode); //---------------
if(lResult != MMC_SUCCESS)
{
LogError("DemoProfileVelocityMode", lResult, lErrorCode);
}
else
{
// lResult = DemoProfilePositionMode(g_pKeyHandle, g_usNodeId, lErrorCode);
// if(lResult != MMC_SUCCESS)
// {
// LogError("DemoProfilePositionMode", lResult, lErrorCode);
// }
// else
// {
if(VCS_SetDisableState(g_pKeyHandle, g_usNodeId, &lErrorCode) == 0) //--------------------
{
LogError("VCS_SetDisableState", lResult, lErrorCode);
lResult = MMC_FAILED;
}
// }
}
return lResult;
}
void PrintHeader() //can clear that..------------------------------------------------------
{ // Epos Command Library Example Program, (c) maxonmotor ag 2014-2017
SeparatorLine(); //---------------------------------------------------------------
LogInfo("Epos Command Library Example Program, (c) maxonmotor ag 2014-2017");
SeparatorLine();
}
/*****************************************************************************/
double linear_temp=0,angular_temp=0;//暂存的线速度和角速度
double set_left_p=0,set_right_p=0;
int set_left_v=0,set_right_v=0;
/************************************************************/
void callback(const geometry_msgs::Twist & cmd_input)//订阅/cmd_vel主题回调函数
{
unsigned int ulErrorCode = 0;
int lResult = MMC_SUCCESS;
unsigned int lErrorCode = 0;
linear_temp = cmd_input.linear.x ;//获取/cmd_vel的线速度.m/s
angular_temp = cmd_input.angular.z ;//获取/cmd_vel的角速度,rad/s
set_left_p=2*214500*linear_temp*0.2-(1875*2*180)*angular_temp*0.2/3.1415926;
set_right_p=2*214500*linear_temp*0.2+(1875*2*180)*angular_temp*0.2/3.1415926;
set_left_v=(int)(set_left_p/(2*164.434*0.65154));
set_right_v=(int)(set_right_p/(2*164.434*0.65154));//最后一定要int化
ROS_INFO("the velocity vx-----the velocity vth-----: [%d]---[%d]",set_left_v,set_right_v);
VCS_MoveWithVelocity(g_pKeyHandle, g_usNodeId, set_left_v*(-1), &lErrorCode); //设置速度 int_linear_temp
VCS_MoveWithVelocity(g_pKeyHandle2, g_usNodeId2, set_right_v, &lErrorCode); //设置2 int_linear_temp
ROS_INFO("this temp v: [%d]",set_left_v);
//-----------------------------------------键盘控制程序------------------------------------------------
/* linear_temp=linear_temp*400; // 最后得到的速度都是200 前进或者后退 或者是别的都是200
angular_temp =angular_temp*200;
// ROS_INFO("I heard: [%f]",linear_temp);
// ROS_INFO("I heard: [%f]",angular_temp);
int int_linear_temp = (int)linear_temp;
int int_angular_temp = (int)angular_temp;
// ROS_INFO("the velocity vx-----the velocity vth-----: [%d]---[%d]",int_linear_temp,int_angular_temp);
if(int_linear_temp!=0&&int_angular_temp!=0)
{
LogInfo("halt velocity movement，yes node1");
VCS_HaltVelocityMovement(g_pKeyHandle, g_usNodeId, &lErrorCode); //禁止速度模式
VCS_SetDisableState(g_pKeyHandle, g_usNodeId, &lErrorCode); //释放标志位
// CloseDevice(&ulErrorCode); // 这里不应该提前写 ，提前写关不了驱动，应该先终止速度模式
ROS_INFO("I heard:CloseDevice,yes node1");
//-------------------------------------------------------------------------------------------------
LogInfo("halt velocity movement，yes node2");
VCS_HaltVelocityMovement(g_pKeyHandle2, g_usNodeId2, &lErrorCode); //禁止速度模式
VCS_SetDisableState(g_pKeyHandle2, g_usNodeId2, &lErrorCode); //释放标志位
CloseDevice(&ulErrorCode);
ROS_INFO("I heard:CloseDevice,yes node2");
}
else if((int_linear_temp!=0)&&(int_angular_temp==0))
{
LogInfo("set velocity ，yes");
VCS_MoveWithVelocity(g_pKeyHandle, g_usNodeId, int_linear_temp*(-1), &lErrorCode); //设置速度 int_linear_temp
VCS_MoveWithVelocity(g_pKeyHandle2, g_usNodeId2, int_linear_temp, &lErrorCode); //设置2 int_linear_temp
ROS_INFO("this temp v: [%d]",int_linear_temp);
}
else if((int_linear_temp==0)&&(int_angular_temp>0))
{
VCS_MoveWithVelocity(g_pKeyHandle, g_usNodeId, int_angular_temp*(1), &lErrorCode); //设置速度-0.5 int_linear_temp
VCS_MoveWithVelocity(g_pKeyHandle2, g_usNodeId2, int_angular_temp*1, &lErrorCode); //设置2 3.5 int_linear_temp
}
else
{
VCS_MoveWithVelocity(g_pKeyHandle, g_usNodeId, int_angular_temp*(1), &lErrorCode); //设置速度3.5 int_linear_temp
VCS_MoveWithVelocity(g_pKeyHandle2, g_usNodeId2, int_angular_temp*(1), &lErrorCode); //设置2 -0.5
}
*/
}
//-------------------------NAV-100的回调函数-------------------------------------------------------------------------
//void messageCallback(const std_msgs::String::ConstPtr& msg4)
//{
//ROS_INFO("I heard: [%s]", msg4->data.c_str());
//}
void logcallback(const sensor_msgs::Joy::ConstPtr& Joy) //订阅方把订阅的话题中的消息发布出来
{
//-----------------罗技手柄的接受程序------目的在于重新编码，便于编程---------------------------------------------------
axis_lin1= Joy->axes[1]; //左边遥感
axis_ang1= Joy->axes[0];
axis_lin2= Joy->axes[4]; //右边摇杆
axis_ang2= Joy->axes[3];
axis_lin3= Joy->axes[7]; //八方向控制
axis_ang3= Joy->axes[6];
ltadd1= Joy->axes[2];
rtadd1= Joy->axes[5];
for(int i=0;i<9;i++)
{
ton[i]=Joy->buttons[i]; //依次A B X Y 0123 LB RB BACK START 4567 LOG 8
}
ROS_INFO("linear1:%f angular1:%f linear2:%f angular2:%f linear3:%f angular3:%f ltadd1:%f rtadd1:%f", axis_lin1,axis_ang1,axis_lin2,axis_ang2,axis_lin3,axis_ang3,ltadd1,rtadd1);
ROS_INFO("bt0:%d bt1:%d bt2:%d bt3:%d bt4:%d bt5:%d bt6:%d bt7:%d bt8:%d", ton[0],ton[1],ton[2],ton[3],ton[4],ton[5],ton[6],ton[7],ton[8]);
//-----------------------------------------------------------------------------------------------
unsigned int ulErrorCode = 0;
int lResult = MMC_SUCCESS;
unsigned int lErrorCode = 0;
if(ton[7]) //start 开启设备
{
OpenDevice(&ulErrorCode);
PrepareDemo(&ulErrorCode);
LogInfo("ActivateProfileVelocityMode node1，yes");
VCS_ActivateProfileVelocityMode(g_pKeyHandle, g_usNodeId, &lErrorCode); //激活速度模式
LogInfo("ActivateProfileVelocityMode node2，yes");
VCS_ActivateProfileVelocityMode(g_pKeyHandle2, g_usNodeId2, &lErrorCode);
}
if(ton[6]) //back 就可以关闭电机驱动器
{
LogInfo("halt velocity movement，yes node1");
VCS_HaltVelocityMovement(g_pKeyHandle, g_usNodeId, &lErrorCode); //禁止速度模式
VCS_SetDisableState(g_pKeyHandle, g_usNodeId, &lErrorCode); //释放标志位
//-------------------------------------------------------------------------------------------------
LogInfo("halt velocity movement，yes node2");
VCS_HaltVelocityMovement(g_pKeyHandle2, g_usNodeId2, &lErrorCode); //禁止速度模式
VCS_SetDisableState(g_pKeyHandle2, g_usNodeId2, &lErrorCode); //释放标志位
CloseDevice(&ulErrorCode);
ROS_INFO("CloseDevice,yes node1");
ROS_INFO("CloseDevice,yes node2");
}
if(ton[5])
{
int lendmotor=0,rendmotor=0;
float log_linear_temp = axis_lin1 ;//获取/cmd_vel的线速度.m/s
float log_angular_temp = axis_ang1 ;//获取/cmd_vel的角速度,rad/s 这里将罗技手柄的摇动杆子1写了进去
log_linear_temp=log_linear_temp*750; // 将0.5的浮点书扩大十倍
log_angular_temp =log_angular_temp*450;
int lt_temp = ltadd1 ; //
int rt_temp = rtadd1 ; //
lt_temp=(1-lt_temp)*200; // 左边减速 右边加速
rt_temp =(1-rt_temp)*200;
int int_log_linear_temp = (int)log_linear_temp;
int int_log_angular_temp = (int)log_angular_temp; //速度的int转化
int int_rt_temp = (int)rt_temp;
int int_lt_temp = (int)lt_temp;
if(int_log_linear_temp>0)
{
lendmotor=int_log_linear_temp*(-1)+int_log_angular_temp-int_rt_temp+int_lt_temp;
rendmotor=int_log_linear_temp+int_log_angular_temp+int_rt_temp-int_lt_temp;
}
else
{
lendmotor=int_log_linear_temp*(-1)-int_log_angular_temp+int_rt_temp-int_lt_temp;
rendmotor=int_log_linear_temp-int_log_angular_temp-int_rt_temp+int_lt_temp;
}
LogInfo("set velocity ，yes");
VCS_MoveWithVelocity(g_pKeyHandle, g_usNodeId, lendmotor, &lErrorCode);
VCS_MoveWithVelocity(g_pKeyHandle2, g_usNodeId2, rendmotor, &lErrorCode);
ROS_INFO("this temp v: [%d] [%d]",lendmotor,rendmotor);
}
}
int main(int argc, char **argv)
{
unsigned int lErrorCode = 0;
int lResult = MMC_FAILED;
unsigned int ulErrorCode = 0;
long diff_fime=0;
double dx =0;
double dy =0;
double dth =0;
double distance_left;
double distance_right;
PrintHeader();
SetDefaultParameters();
if((lResult = ParseArguments(argc, argv))!=MMC_SUCCESS)
{
return lResult;
}
PrintSettings(); //output *efault settings: example protocal stack name = 'MAXON_RS232
if((lResult = OpenDevice(&ulErrorCode))!=MMC_SUCCESS)
{
LogError("OpenDevice", lResult, ulErrorCode);
return lResult;
}
PrepareDemo(&ulErrorCode);
LogInfo("ActivateProfileVelocityMode node1，yes");
VCS_ActivateProfileVelocityMode(g_pKeyHandle, g_usNodeId, &lErrorCode); //激活速度模式
LogInfo("ActivateProfileVelocityMode node2，yes");
VCS_ActivateProfileVelocityMode(g_pKeyHandle2, g_usNodeId2, &lErrorCode);
//-----------------------------------
ros::init(argc, argv, "base_controller"); //初始化串口节点
ros::NodeHandle n; //定义节点进程句柄
ros::Subscriber sub = n.subscribe("cmd_vel", 200, callback); //订阅/cmd_vel主题
ros::Subscriber log_sub = n.subscribe<sensor_msgs::Joy>("joy",10,logcallback); //订阅游戏手柄发来的数据
// ------------------------------------------------
// ros::Subscriber sub1 = n.subscribe("message", 1000, messageCallback); // 这里的sub1是什么意思
ros::Publisher msg_pub = n.advertise<car_msgs::carspeed>("car_msgs", 1000);
ros::Rate loop_rate(50);
while (ros::ok())
{
VCS_GetPositionIs(g_pKeyHandle, g_usNodeId, &actual_p_left, &lErrorCode);
VCS_GetPositionIs(g_pKeyHandle2, g_usNodeId2, &actual_p_right, &lErrorCode);
// ROS_INFO("main_actual left_position---right_position: [%d]---[%d]",actual_p_left,actual_p_right);
//---------------------------左右脉冲的变化量------------------------这里按着前进固定为3000---------------------------------------
diff_actual_p_left=actual_p_left-last_actual_p_left;
diff_actual_p_right=actual_p_right-last_actual_p_right;
// ROS_INFO("main_actual_p_left_difference---actual_p_right_difference: [%d]---[%d]",diff_actual_p_left,diff_actual_p_right);
last_actual_p_left=actual_p_left;
last_actual_p_right=actual_p_right;
//--------------------------记录当前的时间---------------------------------------------------------------------------
gettimeofday(&timenow_value,NULL);
// ROS_INFO("main_timenow_value----------::::::::::::::: [%ld]s",timenow_value.tv_sec);
diff_fime=timenow_value.tv_sec*1000 + timenow_value.tv_usec/1000-lasttime;
// ROS_INFO("main_time_difference_value----------------- [%ld]ms",diff_fime);
lasttime=timenow_value.tv_sec*1000 + timenow_value.tv_usec/1000;
//这里已经知道了两个轮子的位置移动 还有时间差 直接发布 在里程计的程序中 直接根据位置差值 计算机器人位置 发布里程‘
// 顺便算出写为vx vy=0 vth 一个线速度 一个角度速度即可
distance_left=(double)diff_actual_p_left/241500;// 这里应该处以一个随速度给定的转化量
//也就是每米走的脉冲数（尽量以速度参数形式给出）
distance_right=(double)diff_actual_p_right/241500; //其实上下是不一样的 为了简洁取了均值
dx=(-distance_left+distance_right)/2.0;//前进的位置 这里虽然没有求出速度vx 但是给出了一小段时间的前进位移
dth=(diff_actual_p_left+diff_actual_p_right)*3.141592653/(2043*2*180); //0.9为机器人的宽 算转过的角度. 这里算角度直接根据实际测量 而不是0.9 试验中发现这个不准 具体愿意未知
dx=dx/diff_fime*1000;
dy=0;
dth=dth/diff_fime*1000; //此处因为这里毫秒 乘以1000直接转化为秒的单位 的米/秒 rad/秒
car_msgs::carspeed msg_v;
msg_v.diff_position.resize(3);
msg_v.diff_position[0]=dx;
msg_v.diff_position[1]=dy;
msg_v.diff_position[2]=dth;
msg_pub.publish(msg_v);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
odometry_publisher_car.cpp
主要是通过vth vy vx 得到th x y
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <car_msgs/carspeed.h>
double vx = 0;
double vy = 0;
double vth =0;
void msgCallback(const car_msgs::carspeed &msg)//订阅/carspeed回调函数
{
vx=msg.diff_position[0];
vy=msg.diff_position[1];
vth=msg.diff_position[2];
}
int main(int argc, char** argv)
{
double x = 0;
double y = 0;
double th = 0;
ros::init(argc, argv, "odometry_publisher_car");
ros::NodeHandle n;
ros::Subscriber odom_sub = n.subscribe("car_msgs", 1000, msgCallback);
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 1000);
ros::Rate r(5);
tf::TransformBroadcaster odom_broadcaster;
ros::Time current_time,last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
while(ros::ok())
{
ros::Time current_time, last_time;
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
//double dt = (current_time - last_time).toSec(); 这里的计算时间有问题因为是单位不对 但是这里本身就5的频率 所以直接给定时间是0.2s
double dt=0.2;
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th += delta_th;
ROS_INFO("vx---vy---vth: [%f]---[%f]---[%f]",vx,vy,vth);
ROS_INFO("x---y---th: [%f]---[%f]---[%f]",x,y,th);
//since all odometry is 6DOF we'll need a quaternion created from yaw
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//set the velocity
odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.angular.z = vth;
//publish the message
odom_pub.publish(odom);
last_time = current_time;
ros::spinOnce(); // check for incoming messages
r.sleep();
}
}
9./home/hao/catkin_lh/src/nav_100
NAV——100的组合导航的ros移植实现
src/nav_100/src/an_packet_protocol.c
src/nav_100/src/mytest.cpp
src/nav_100/src/packet_example.cpp
src/nav_100/src/rs232.c
src/nav_100/src/spatial_packets.c
catkin_lh/src/nav_100/include/nav_100/an_packet_protocol.h
catkin_lh/src/nav_100/include/nav_100/rs232.h
catkin_lh/src/nav_100/include/nav_100/spatial_packets.h
packet_example.cpp
/****************************************************************/
/* */
/* Advanced Navigation Packet Protocol Library */
/* C Language Dynamic Spatial SDK, Version 4.0 */
/* Copyright 2014, Xavier Orr, Advanced Navigation Pty Ltd */
/* */
/****************************************************************/
/*
* Copyright (C) 2014 Advanced Navigation Pty Ltd
*
* Permission is hereby granted, free of charge, to any person obtaining
* a copy of this software and associated documentation files (the "Software"),
* to deal in the Software without restriction, including without limitation
* the rights to use, copy, modify, merge, publish, distribute, sublicense,
* and/or sell copies of the Software, and to permit persons to whom the
* Software is furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included
* in all copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
* OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
* FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
* DEALINGS IN THE SOFTWARE.
*/
#include <stdlib.h>
#include <stdio.h>
#include <stdint.h>
#include <math.h>
#ifdef _WIN32
#include <Windows.h>
#else
#include <unistd.h>
#endif
#include "rs232.h"
#include "an_packet_protocol.h"
#include "spatial_packets.h"
//-----------------------------------
#include "ros/ros.h"
#include <sstream>
#include "std_msgs/Float32MultiArray.h"
#include <car_msgs/nav_100.h>
//------------------------------------
#define RADIANS_TO_DEGREES (180.0/M_PI)
int an_packet_transmit(an_packet_t *an_packet)
{
an_packet_encode(an_packet);
return SendBuf(an_packet_pointer(an_packet), an_packet_size(an_packet));
}
/*
* This is an example of sending a configuration packet to Spatial.
*
* 1. First declare the structure for the packet, in this case sensor_ranges_packet_t.
* 2. Set all the fields of the packet structure
* 3. Encode the packet structure into an an_packet_t using the appropriate helper function
* 4. Send the packet
* 5. Free the packet
*/
void set_sensor_ranges()
{
an_packet_t *an_packet;
sensor_ranges_packet_t sensor_ranges_packet;
sensor_ranges_packet.permanent = TRUE;
sensor_ranges_packet.accelerometers_range = accelerometer_range_4g;
sensor_ranges_packet.gyroscopes_range = gyroscope_range_500dps;
sensor_ranges_packet.magnetometers_range = magnetometer_range_2g;
an_packet = encode_sensor_ranges_packet(&sensor_ranges_packet);
an_packet_transmit(an_packet);
an_packet_free(&an_packet);
}
int main(int argc, char **argv)
{
//*******************************************************************************************//
an_decoder_t an_decoder;
an_packet_t *an_packet;
system_state_packet_t system_state_packet;
raw_sensors_packet_t raw_sensors_packet;
int bytes_received;
// if (argc != 3)
// {
// printf("Usage - program com_port baud_rate\nExample - packet_example.exe COM1 115200\n");
// exit(EXIT_FAILURE);
// }
//------------------------------------------------------------------ 在这里更改端口名字和波特率
/* open the com port */
if (OpenComport("/dev/ttyUSB2",115200)) //if (OpenComport(argv[1], atoi(argv[2]))) /dev/ttyUSB0,115200
//这里的更改很完美 晚上加鸡腿 nice
{
printf("Could not open serial port\n");
exit(EXIT_FAILURE);
}
//-----------------------------------------------------------------------------
an_decoder_initialise(&an_decoder);
//*********************************************************************************************************//
ros::init(argc, argv, "navigation");
ros::NodeHandle n;
ros::Publisher pub = n.advertise<car_msgs::nav_100>("car_orientation", 1000);
ros::Rate loop_rate(20); // 这里原来是10
while (ros::ok())
{
//------------------------------------------------------------------------------------------------
if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0)
{
/* increment the decode buffer length by the number of bytes received */
an_decoder_increment(&an_decoder, bytes_received);
/* decode all the packets in the buffer */
while ((an_packet = an_packet_decode(&an_decoder)) != NULL)
{
if (an_packet->id == packet_id_system_state) /* system state packet */
{
/* copy all the binary data into the typedef struct for the packet */
/* this allows easy access to all the different values */
if(decode_system_state_packet(&system_state_packet, an_packet) == 0)
{
// printf("System State Packet:\n");
// printf("\tLatitude = %f, Longitude = %f, Height = %f\n", system_state_packet.latitude * RADIANS_TO_DEGREES, system_state_packet.longitude * RADIANS_TO_DEGREES, system_state_packet.height);
// printf("\tRoll = %f, Pitch = %f, Heading = %f\n", system_state_packet.orientation[0] * RADIANS_TO_DEGREES, system_state_packet.orientation[1] * RADIANS_TO_DEGREES, system_state_packet.orientation[2] * RADIANS_TO_DEGREES);
}
}
else if (an_packet->id == packet_id_raw_sensors) /* raw sensors packet */
{
/* copy all the binary data into the typedef struct for the packet */
/* this allows easy access to all the different values */
if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0)
{
// printf("Raw Sensors Packet:\n");
// printf("\tAccelerometers X: %f Y: %f Z: %f\n", raw_sensors_packet.accelerometers[0], raw_sensors_packet.accelerometers[1], raw_sensors_packet.accelerometers[2]);
// printf("\tGyroscopes X: %f Y: %f Z: %f\n", raw_sensors_packet.gyroscopes[0] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[1] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[2] * RADIANS_TO_DEGREES);
}
}
else
{
printf("Packet ID %u of Length %u\n", an_packet->id, an_packet->length);
}
/* Ensure that you free the an_packet when your done with it or you will leak memory */
an_packet_free(&an_packet);
}
}
#ifdef _WIN32
Sleep(10);
#else
usleep(10000); //10ms
#endif
//-------------------------------------------------------
car_msgs::nav_100 msg;
msg.nav_100.resize(15);
msg.nav_100[0]=system_state_packet.latitude * RADIANS_TO_DEGREES;
msg.nav_100[1]=system_state_packet.longitude * RADIANS_TO_DEGREES;
msg.nav_100[2]=system_state_packet.height;
msg.nav_100[3]=system_state_packet.orientation[0] * RADIANS_TO_DEGREES;
msg.nav_100[4]=system_state_packet.orientation[1] * RADIANS_TO_DEGREES;
msg.nav_100[5]=system_state_packet.orientation[2] * RADIANS_TO_DEGREES;
msg.nav_100[6]=raw_sensors_packet.accelerometers[0];
msg.nav_100[7]=raw_sensors_packet.accelerometers[1];
msg.nav_100[8]=raw_sensors_packet.accelerometers[2];
msg.nav_100[9]=raw_sensors_packet.gyroscopes[0] * RADIANS_TO_DEGREES;
msg.nav_100[10]=raw_sensors_packet.gyroscopes[1] * RADIANS_TO_DEGREES;
msg.nav_100[11]=raw_sensors_packet.gyroscopes[2] * RADIANS_TO_DEGREES;
msg.nav_100[12]=system_state_packet.velocity[0] ;
msg.nav_100[13]=system_state_packet.velocity[1] ;
msg.nav_100[14]=system_state_packet.velocity[2] ;
ROS_INFO("latitude %f longitude %f height %f----Roll %f Pitch %f Heading %f",msg.nav_100[0],msg.nav_100[1],msg.nav_100[2],msg.nav_100[3],msg.nav_100[4],msg.nav_100[5]);
ROS_INFO("Accelerometers X: %f Y: %f Z: %f----Gyroscopes X: %f Y: %f Z: %f",msg.nav_100[6],msg.nav_100[7],msg.nav_100[8],msg.nav_100[9],msg.nav_100[10],msg.nav_100[11]);
ROS_INFO("velocity X: %f Y: %f Z: %f",msg.nav_100[12],msg.nav_100[13],msg.nav_100[14]);
pub.publish(msg);
//------------------------------------------------------------
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
10./home/hao/catkin_lh/src/navigation
来源于ros.wiki的导航栈，加载了很多导航需要的功能包，如movebase sever地图等等，直接下载无需编译
3346
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
