电应普1托4超声波在ros中的使用

最新推荐文章于 2024-06-17 20:59:17 发布

唯依/808

最新推荐文章于 2024-06-17 20:59:17 发布

阅读量472

点赞数 1

文章标签：自动驾驶 c++ c语言

本文链接：https://blog.csdn.net/qq_42485734/article/details/125043803

版权

1最近无人机避障准备增加超声波试试

2.操作：

建立一个包把下列文件放到对应位置编译。

头文件放include 消息msg cpp放src cmakelist和package放包里面

头文件

comm_service.h

#ifndef COMM_SERVICE_H
#define COMM_SERVICE_H

#include <ros/ros.h>

#define HEADER    0xff		//数据头

typedef unsigned char      u8;
typedef unsigned short int u16;
typedef short int          s16;
typedef unsigned       int u32;


union ReturnRobotData
{
   u8 data[10];
   struct
   {
      u8 header1;
    /*   u8 header2;
      u8 Add;
      u8 Cmd; */
      u8 Data_1H;
      u8 Data_1L;
      u8 Data_2H;
      u8 Data_2L;
      u8 Data_3H;
      u8 Data_3L;
      u8 Data_4H;
      u8 Data_4L;
      u8 Checksum;
   }prot;
}ReturnRobotData;

#endif // ROS_COM_MSG_H

ros_com_msg.h

// Generated by gencpp from file nb/ros_com_msg.msg
// DO NOT EDIT!


#ifndef NB_MESSAGE_ROS_COM_MSG_H
#define NB_MESSAGE_ROS_COM_MSG_H


#include <string>
#include <vector>
#include <map>

#include <ros/types.h>
#include <ros/serialization.h>
#include <ros/builtin_message_traits.h>
#include <ros/message_operations.h>


namespace nb
{
template <class ContainerAllocator>
struct ros_com_msg_
{
  typedef ros_com_msg_<ContainerAllocator> Type;

  ros_com_msg_()
    : Data_1(0)
    , Data_2(0)
    , Data_3(0)
    , Data_4(0)  {
    }
  ros_com_msg_(const ContainerAllocator& _alloc)
    : Data_1(0)
    , Data_2(0)
    , Data_3(0)
    , Data_4(0)  {
  (void)_alloc;
    }



   typedef int16_t _Data_1_type;
  _Data_1_type Data_1;

   typedef int16_t _Data_2_type;
  _Data_2_type Data_2;

   typedef int16_t _Data_3_type;
  _Data_3_type Data_3;

   typedef int16_t _Data_4_type;
  _Data_4_type Data_4;





  typedef boost::shared_ptr< ::nb::ros_com_msg_<ContainerAllocator> > Ptr;
  typedef boost::shared_ptr< ::nb::ros_com_msg_<ContainerAllocator> const> ConstPtr;

}; // struct ros_com_msg_

typedef ::nb::ros_com_msg_<std::allocator<void> > ros_com_msg;

typedef boost::shared_ptr< ::nb::ros_com_msg > ros_com_msgPtr;
typedef boost::shared_ptr< ::nb::ros_com_msg const> ros_com_msgConstPtr;

// constants requiring out of line definition



template<typename ContainerAllocator>
std::ostream& operator<<(std::ostream& s, const ::nb::ros_com_msg_<ContainerAllocator> & v)
{
ros::message_operations::Printer< ::nb::ros_com_msg_<ContainerAllocator> >::stream(s, "", v);
return s;
}


template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator==(const ::nb::ros_com_msg_<ContainerAllocator1> & lhs, const ::nb::ros_com_msg_<ContainerAllocator2> & rhs)
{
  return lhs.Data_1 == rhs.Data_1 &&
    lhs.Data_2 == rhs.Data_2 &&
    lhs.Data_3 == rhs.Data_3 &&
    lhs.Data_4 == rhs.Data_4;
}

template<typename ContainerAllocator1, typename ContainerAllocator2>
bool operator!=(const ::nb::ros_com_msg_<ContainerAllocator1> & lhs, const ::nb::ros_com_msg_<ContainerAllocator2> & rhs)
{
  return !(lhs == rhs);
}


} // namespace nb

namespace ros
{
namespace message_traits
{





template <class ContainerAllocator>
struct IsFixedSize< ::nb::ros_com_msg_<ContainerAllocator> >
  : TrueType
  { };

template <class ContainerAllocator>
struct IsFixedSize< ::nb::ros_com_msg_<ContainerAllocator> const>
  : TrueType
  { };

template <class ContainerAllocator>
struct IsMessage< ::nb::ros_com_msg_<ContainerAllocator> >
  : TrueType
  { };

template <class ContainerAllocator>
struct IsMessage< ::nb::ros_com_msg_<ContainerAllocator> const>
  : TrueType
  { };

template <class ContainerAllocator>
struct HasHeader< ::nb::ros_com_msg_<ContainerAllocator> >
  : FalseType
  { };

template <class ContainerAllocator>
struct HasHeader< ::nb::ros_com_msg_<ContainerAllocator> const>
  : FalseType
  { };


template<class ContainerAllocator>
struct MD5Sum< ::nb::ros_com_msg_<ContainerAllocator> >
{
  static const char* value()
  {
    return "658383f09741f4ed491c8bc370a1c542";
  }

  static const char* value(const ::nb::ros_com_msg_<ContainerAllocator>&) { return value(); }
  static const uint64_t static_value1 = 0x658383f09741f4edULL;
  static const uint64_t static_value2 = 0x491c8bc370a1c542ULL;
};

template<class ContainerAllocator>
struct DataType< ::nb::ros_com_msg_<ContainerAllocator> >
{
  static const char* value()
  {
    return "nb/ros_com_msg";
  }

  static const char* value(const ::nb::ros_com_msg_<ContainerAllocator>&) { return value(); }
};

template<class ContainerAllocator>
struct Definition< ::nb::ros_com_msg_<ContainerAllocator> >
{
  static const char* value()
  {
    return "\n"
"int16 Data_1\n"
"int16 Data_2\n"
"int16 Data_3\n"
"int16 Data_4\n"
"\n"
;
  }

  static const char* value(const ::nb::ros_com_msg_<ContainerAllocator>&) { return value(); }
};

} // namespace message_traits
} // namespace ros

namespace ros
{
namespace serialization
{

  template<class ContainerAllocator> struct Serializer< ::nb::ros_com_msg_<ContainerAllocator> >
  {
    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
    {
      stream.next(m.Data_1);
      stream.next(m.Data_2);
      stream.next(m.Data_3);
      stream.next(m.Data_4);
    }

    ROS_DECLARE_ALLINONE_SERIALIZER
  }; // struct ros_com_msg_

} // namespace serialization
} // namespace ros

namespace ros
{
namespace message_operations
{

template<class ContainerAllocator>
struct Printer< ::nb::ros_com_msg_<ContainerAllocator> >
{
  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::nb::ros_com_msg_<ContainerAllocator>& v)
  {
    s << indent << "Data_1: ";
    Printer<int16_t>::stream(s, indent + "  ", v.Data_1);
    s << indent << "Data_2: ";
    Printer<int16_t>::stream(s, indent + "  ", v.Data_2);
    s << indent << "Data_3: ";
    Printer<int16_t>::stream(s, indent + "  ", v.Data_3);
    s << indent << "Data_4: ";
    Printer<int16_t>::stream(s, indent + "  ", v.Data_4);
  }
};

} // namespace message_operations
} // namespace ros

#endif // NB_MESSAGE_ROS_COM_MSG_H

消息ros_com_msg.msg

int16 Data_1
int16 Data_2
int16 Data_3
int16 Data_4

.c文件

#include <nb/comm_service.h>
#include <ros_com_msg.h>
#include <ros/ros.h>                      //类似 C 语言的 stdio.h
#include <serial/serial.h>                //ROS已经内置了的串口包
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <signal.h>
#include <sensor_msgs/Imu.h> 
#include <ros/duration.h>
#include <string.h> 
 
serial::Serial ser; //声明串口对象

bool sendcheck = true ;

unsigned char all_data[5] = {0x55,0xAA,0x01,0x01,0x01}; //声明发出指令(帧头(0x55,0xAA)，地址(0x01、0x02、0x03)，指令(0x10)，checksum)


const int MAX = 100;
int DecArr[MAX] = { 0 };

//十六进制转十进制
int Hex_Conversion_Dec(int aHex)
{
    long Dec = 0;
    int temp = 0;
    int count = 0;

    while (0 != aHex)//循环直至aHex的商为零
    {
//        ROS_INFO("%d------------------------------\n",aHex%16);
        temp = aHex;
        aHex = aHex / 16;//求商
        temp = temp % 16;//取余
        DecArr[count++] = temp;//余数存入数组
    }
    int j = 0;
    for (int i = 0; i<count; i++ )
    {
        if (i < 1)
        {
            Dec = Dec + DecArr[i];
        }
        else
        {
        //16左移4位即16²，左移8位即16³、以此类推。
            Dec = (Dec + (DecArr[i]*(16<<j)));
            j += 4;
        }
    }
    return Dec ;
}

/*************************************************************************/
//当关闭包时调用，关闭上传
void mySigIntHandler(int sig)
{
   ROS_INFO("close the com serial!\n");
   ser.close();
   ros::shutdown();
}
/*************************************************************************/

void timercallback(const ros::TimerEvent&)
{
    ser.write(all_data,5);
//    ROS_INFO("write----------\n");
}

void cmd_comCallback(const nb::ros_com_msg &msg)
{
    ROS_INFO("Subcribe Person Info++++++++++++++++: data1:%d  data2:%d  data3:%d  data4:%d\n",
                msg.Data_1, msg.Data_2, msg.Data_3,msg.Data_4);
}

int main(int argc,char **argv)
{
   u16 len = 0;
//   u16 TempCheck = 0 , check =0;
   u8  data[200];
   std::string usart_port;

   ros::init(argc,argv,"ultrasonic_talker",ros::init_options::NoSigintHandler);            //解析参数，命名节点为 com_talker
   nb::ros_com_msg  com_msg;
   ros::NodeHandle nh;
   ros::Publisher pub = nh.advertise<nb::ros_com_msg>("ultrasonic",100);            //创建一个publisher对象，发布名为com_info，队列长度为1，消息类型为ros::com_msg::com。
   signal(SIGINT, mySigIntHandler);  											//把原来ctrl+c中断函数覆盖掉，把信号槽连接到mySigIntHandler保证关闭节点
   //ros::Timer timer1 = nh.createTimer(ros::Duration (0.25), timercallback);    //设置定时器每隔250ms发送数据
   ros::Subscriber sub = nh.subscribe("cmd_com",1,cmd_comCallback);  //创建subscriber 对象，名为cmd_com,注册回调函数为cmd_velCallback，队列长度为1。

   try
    {
       //设置串口属性，并打开串口
      ser.setPort("/dev/ttyUSB0");
      ser.setBaudrate(9600);
      serial::Timeout to = serial::Timeout::simpleTimeout(1000);
      ser.setTimeout(to);
      ser.open();
    }
    catch (serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port ");
        return -1;
    }

    //检测串口是否已经打开，并给出提示信息
    if(ser.isOpen())
    {
        ser.flushInput();      //清空输入缓存,把多余的无用数据删除
        ROS_INFO_STREAM("cgq Serial Port initialized");
    }
    else
    {
        return -1;
    }
     ros::Rate loop_rate(100);   //设置循环的频率为4Hz，250ms

   while(ros::ok())
   {

        ser.read(ReturnRobotData.data,sizeof(ReturnRobotData.data));
        u16 TempCheck = 0x00 , check =0;
        char num1 =0 ,num2 =0, num3 = 0,num4 =0;
        for(u8 i=0 ; i<sizeof(ReturnRobotData.data)-1; i++)
        {
         TempCheck += ReturnRobotData.data[i];
         check = TempCheck & 0xff;
        }
        //验证checksum
    /*printf("TempCheck :%x\n",TempCheck);
        printf("Returnchecksum :%x\n",ReturnRobotData.prot.Checksum);
        printf("Returncheck :%X\n",check);
*/
    if(ReturnRobotData.prot.header1 == 0xff  && ReturnRobotData.prot.Checksum == check)
        {
            ROS_INFO_STREAM("read seriel------------------");

           int num1 = ReturnRobotData.prot.Data_1H*256 + ReturnRobotData.prot.Data_1L<250 ? 4500:ReturnRobotData.prot.Data_1H*256 + ReturnRobotData.prot.Data_1L;
           int num2 = ReturnRobotData.prot.Data_2H*256 + ReturnRobotData.prot.Data_2L<250 ? 4500:ReturnRobotData.prot.Data_2H*256 + ReturnRobotData.prot.Data_2L;
           int num3 = ReturnRobotData.prot.Data_3H*256 + ReturnRobotData.prot.Data_3L<250 ? 4500:ReturnRobotData.prot.Data_3H*256 + ReturnRobotData.prot.Data_3L;
           int num4 = ReturnRobotData.prot.Data_4H*256 + ReturnRobotData.prot.Data_4L<250 ? 4500:ReturnRobotData.prot.Data_4H*256 + ReturnRobotData.prot.Data_4L;
         //   printf("ultrasonic_num1（0x）:%X ultrasonic_num1（0x）:%X ultrasonic_num3（0x）:%X ultrasonic_num4（0x）:%X\n",num1,num2,num3,num4);
            //十六进制转化为十进制
            int zhuanhuanum1 =  Hex_Conversion_Dec(num1);
            int zhuanhuanum2 =  Hex_Conversion_Dec(num2);
            int zhuanhuanum3 =  Hex_Conversion_Dec(num3);
            int zhuanhuanum4 =  Hex_Conversion_Dec(num4);
          //  printf("ultrasonic_num1（mm）:%d ultrasonic_num2（mm）:%d ultrasonic_num3（mm）:%d ultrasonic_num4（mm）:%d \n", zhuanhuanum1,zhuanhuanum2,zhuanhuanum3,zhuanhuanum4);

            com_msg.Data_1 = zhuanhuanum1;
            com_msg.Data_2 = zhuanhuanum2;
            com_msg.Data_3 = zhuanhuanum3;
            com_msg.Data_4 = zhuanhuanum4;
            pub.publish(com_msg); //发布

            ROS_INFO("------------------------------------------------\n");
            memset(ReturnRobotData.data,0,sizeof(ReturnRobotData.data));
        }
        else
        {
//         ROS_INFO("not return %x  %x  %x\n",ReturnRobotData.prot.header1,ReturnRobotData.prot.header2,ReturnRobotData.prot.Checksum);
            ROS_INFO("not read accuracy");
          len = ser.available();
          //清空数据残余
          if(len > 0)
          {
             ser.read(data,len);
          }
          //全部探头打开
        }
      ros::spinOnce();
      loop_rate.sleep();   //按前面设置的4Hz频率将程序挂起
    }
    return 0;
}

cMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(nb)


find_package(catkin REQUIRED COMPONENTS
  nav_msgs
  roscpp
  rospy
  sensor_msgs
  std_msgs
  tf 
  pcl_ros
  message_generation
  serial
  geometry_msgs
)

add_message_files(
  FILES
  ros_com_msg.msg
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(

CATKIN_DEPENDS message_runtime roscpp  std_msgs
) 

include_directories(
 include
 include/nb
  ${catkin_INCLUDE_DIRS}
)

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

package.xml

<?xml version="1.0"?>
<package format="2">
  <name>nb</name>
  <version>0.0.0</version>
  <description>The nb package</description>

  <!-- One maintainer tag required, multiple allowed, one person per tag -->
  <!-- Example:  -->
  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
  <maintainer email="b@todo.todo">b</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/nb</url> -->


  <!-- Author tags are optional, multiple are allowed, one per tag -->
  <!-- Authors do not have to be maintainers, but could be -->
  <!-- Example: -->
  <!-- <author email="jane.doe@example.com">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>nav_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>tf</build_depend>
  <build_export_depend>nav_msgs</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>rospy</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>std_msgs</build_export_depend>
  <build_export_depend>tf</build_export_depend>
  <exec_depend>nav_msgs</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>rospy</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>tf</exec_depend>


     <build_depend>message_generation</build_depend> 
    <exec_depend>message_generation</exec_depend> 
         <build_depend>pcl_ros</build_depend> 
    <exec_depend>pcl_ros</exec_depend> 
         <build_depend>message_runtime </build_depend> 
    <exec_depend>message_runtime </exec_depend> 



  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>