尝试cartographer中使用scan和odom来建图，失败（可能实验室的中继机器人odom有问题）总结

1、在提供中继机器人的代码中修改

modbus协议代码修改：

SineBotVel get_vel()
{
     
    SineBotVel bot_vel;
    memset(&bot_vel,sizeof(bot_vel),0);

    if (m_modbus == NULL) {
     
        return bot_vel;
    }

    int ret;

    float vel[2] = {
     0.0};

    while(ret !=4)  //添加
    {
       
    ret = modbus_read_input_registers(m_modbus, LINEAR_VEL_X_ADDR, 4, (uint16_t *)&vel); //读入数据，存到vel中
    }
    ret = modbus_read_input_registers(m_modbus, LINEAR_VEL_X_ADDR, 4, (uint16_t *)&vel);

    // if(ret == 4)
    // {
     
        bot_vel.linear_vel_x.vel = vel[0];
        bot_vel.angular_vel_z.vel = vel[1];
        bot_vel.linear_vel_y.vel = 0;
    // }
    // else
    // {
     
    //     ROS_INFO("vel recive ret : %d \r\n" ,ret);
    // }

    return bot_vel; //结构体中的xyz值
}

此时机器人的odom解决，里程计可以正常读数，不会出现之前的nan值，x，y值都是正确的。

2、tf变换

之前在turtlebot中robotmodel加载失败，这里在中继机器人中添加urdf文件和tf变换文件。
urdf文件建立（四轮）：

chuan_urtf_xx.launch:

<?xml version="1.0"?>
<launch>

  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find chuan_bringup)/urdf/chuan_xx.urdf.xacro'"/>
  <param name="use_gui" value="False"/>
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"></node>
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" output="screen">
    <param name="publish_frequency" type="double" value="30.0"/>
    <!--param name="tf_prefix" type="string" value="" /-->
  </node>

</launch>

chuan_xx.urdf.xacro:

<?xml version="1.0"?>
  
<robot name="chuan_xx" xmlns:xacro="http://ros.org/wiki/xacro">


  <xacro:include filename="$(find chuan_bringup)/urdf/properties_xx.urdf.xacro" />
  <xacro:include filename="$(find chuan_bringup)/urdf/chuan.urdf.xacro" />
  <xacro:include filename="$(find chuan_bringup)/urdf/basefoot_xx.urdf.xacro" />

</robot>

properties_xx.urdf.xacro:

<?xml version="1.0"?>
<robot xmlns:xacro="http://ros.org/wiki/xacro">

    <material name="blue">
    <color rgba="0 0 0.8 0.5"/>
    </material>

    <material name="black">
        <color rgba="0 0 0 1"/>
    </material>
    
    <material name="white">
        <color rgba="1 1 1 0.5"/>
    </material>
    <material name="fense">
        <color rgba="0.3 0 0.3 0.8"/>
    </material>
</robot>

chuan.urdf.xacro:

<?xml version="1.0"?>
<robot name="chuan" xmlns:xacro="http://ros.org/wiki/xacro">
  
    <link name="base_link">
        <visual>
        <geometry>
            <box size="0.7 0.3 0.3"/>
        </geometry>
        <material name="fense"/>
        </visual>
    </link> 

    <link name="laser">   <!--velodyne -->
        <visual>
        <geometry>
            <cylinder length="0.07" radius="0.05"/>
        </geometry>
        <material name="white"/>
        </visual>
    </link>

    <joint name="laser_to_base_link" type="fixed">   <!--velodyne_to_base_link -->
        <parent link="base_link"/>
        <child link="laser"/>   <!--velodyne -->
        <origin rpy="0 0 0" xyz="0.3 0.0 0.1"/>
    </joint>
</robot>

basefoot_xx.urdf.xacro:

<?xml version="1.0"?> 
<robot name="chuan_xx" xmlns:xacro="http://ros.org/wiki/xacro">



<!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin,
         navigation stack dedpends on this frame -->
  <link name="base_footprint">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
             surface of the collision box matches the top surface of the PR2 base -->
      <origin rpy="0 0 0" xyz="0 0 0.071"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.051"/>
    <child link="base_link"/>
    <parent link="base_footprint"/>
  </joint>

</robot>

tf变换代码：map到odom到base_footprint

tf_odom.launch:

<launch>

    <!-- gps reading Node-->
 
 <!-- <node pkg="chuan_bringup" type="odom_fake" name="odom_fake" output="screen"/>  -->


<node pkg="chuan_bringup" type="tf_changemap11" name="odom_basefootprint" />

<node pkg="chuan_bringup" type="tf_changemaptd11" name="odom_map" />
  
  
</launch>

tf_changemap11.cpp:

#include 
#include 
#include 

int control=0;
double initeasting;
float du;
double initnorthing;

double getRoll(double x,double y,double z, double w)
{
     
double roll=atan2(2*(w*x+y*z),1-2*(x*x+y*y));
return roll;
}


double getPaw(double x,double y,double z, double w)
{
     
double yaw=asin(2*(w*y-x*z));
return yaw;
}

double getYitch(double x,double y,double z, double w)
{
     
double pitch=atan2(2*(w*z+x*y),1-2*(y*y+z*z));
return pitch;
}

void odom_callback(const nav_msgs::OdometryConstPtr& msg)
{
     	
	tf::Quaternion q;
	// 获取偏航角
	float y1=getYitch(msg->pose.pose.orientation.x,msg->pose.pose.orientation.y,msg->pose.pose.orientation.z,msg->pose.pose.orientation.w);
	float y2=0.9;
	du=y1*180.0/3.14159;
	q.setRPY(0, 0, y1);//0.8529 //转化到tf::Quaternion q 下
	printf("%fy d%f\n",y1,du);


	static  tf::TransformBroadcaster br;
	tf::Transform transform;
	transform.setOrigin( tf::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, 0.0) );  //当前机器人的位置（已经做过相对原点位置转换）
	//tf::Quaternion q;
	// q.setRPY(0, 0, -y1);
	//transform.setRotation( tf::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y,msg->pose.pose.orientation.z, msg->pose.pose.orientation.w) );
	transform.setRotation(q);
	br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_footprint"));//odom

//下面修改：
	// tf::Quaternion q;
	// static  tf::TransformBroadcaster br;
	// tf::Transform transform;

	// transform.setOrigin( tf::Vector3(0.0,0.0,0.0) );
	// q.setRPY(0, 0,0.0 );
	// transform.setRotation(q);
	// br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_footprint"));//odom

}


int main(int argc, char** argv)
{
     

ros::init(argc, argv, "robot_tf_publisher");

ros::NodeHandle n;
// ros::Rate rate(51.0);
ros::Subscriber odom_sub = n.subscribe("/odom", 300 , odom_callback);//odom 10--100

ros::spin();
return 0;
};

tf_changemaptd11.cpp:

#include 
#include 
 
int main(int argc, char** argv)
{
     
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;
 
  tf::TransformBroadcaster br;
  tf::Transform transform;
  tf::Quaternion q;
  ros::Rate rate(100);//node.ok()41
  while (ros::ok())
  {
     
    transform.setOrigin( tf::Vector3(0.0,0.0,0.0) );
      //transform.setRotation( tf::Quaternion(0, 0, 0, 1) );-1.1,-1.4, 0.0
    q.setRPY(0, 0,0.0 );//-0.035  -0.175 1.5708  -0.8529
    transform.setRotation(q);
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "odom"));   //odom相对于map是不动的(理想情况)
    rate.sleep();
  }
  return 0;
};

CMakeLists.txt:

add_executable(tf_changemap11 src/tf_changemap11.cpp)
target_link_libraries(tf_changemap11
  ${
     catkin_LIBRARIES}
)


add_executable(tf_changemaptd11 src/tf_changemaptd11.cpp)
target_link_libraries(tf_changemaptd11
  ${
     catkin_LIBRARIES}
)

运行：

roslaunch chuan_bringup chuan_urtf_xx.launch
roslaunch chuan_bringup tf_odom.launch

上面就发布了机器人的laser，base_link，base_footprint的关系，还有odom，map的关系，如下图：

在有正确的odom数据情况下，会有odom到base_footprint的关系，关系图如下：

上面说明正确性，具体情况还要加以讨论，毕竟初学。

3、修改lua参数

在demo_hokuyo.lua中修改

注：这里是失败的，后面在turtlebot上调，暂时先总结

map_frame = "map",    --父框架
  tracking_frame = "odom",   --SLAM算法跟踪帧，imu为“imu_link”
  published_frame = "odom", --发布帧base_footprint
  odom_frame = "odom",
  provide_odom_frame = false,
  publish_frame_projected_to_2d = false,
  use_odometry = true,

4、运行代码

source install_isolated/setup.bash
roslaunch cartographer_ros demo_hokuyo.launch

5、建图效果

节点关系：

建的地图：
这里出现了重影，失败。

总结

明确一点，最终目标建图。

这只是尝试cartographer加上sacn和odom建图，之前上一次博客做了cartographer和激光雷达建图，效果已出，没有在比较的情况和数据的支撑下，不能说明建的地图好坏，是否可以使用。

这里加上odom是为了“一时兴起”，也是学习。在中继机器人太难了，后面有时间在turtlebot平台上跑cartographer建图。

后面的turtlebot上，注意lua参数配置。

注意看topic和tf_tree，这能更好直观的学习。