ROS_Robot/wpb_home
1
0
Fork 0
mirror of https://github.com/6-robot/wpb_home.git synced 2025-09-15 12:58:59 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

add ad&io

Browse Source
This commit is contained in:
zhangwanjie 2021-01-18 17:06:36 +08:00
parent dd5ae26a4d
commit bff8e8d6b1
2 changed files with 42 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
wpb_home_bringup/src/driver/WPB_Home_driver.cpp
View File

@ -167,6 +167,19 @@ void CWPB_Home_driver::m_ParseFrame()
nParseCount = 0;
if (m_ParseBuf[4] == 0x06) //IO
{
unsigned char bIOFlag = 0x01;
for(int i=0;i<4;i++)
{
if((m_ParseBuf[8] & bIOFlag) > 0)
{
arValIOInput[i] = 1;
}
else
{
arValIOInput[i] = 0;
}
bIOFlag = bIOFlag <<1;
}
}
if (m_ParseBuf[4] == 0x07) //AD

29
wpb_home_bringup/src/wpb_home_core.cpp
View File

@ -43,11 +43,16 @@
#include <geometry_msgs/Twist.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/String.h>
#include <std_msgs/Int32MultiArray.h>
#include "driver/WPB_Home_driver.h"
#include <math.h>
static CWPB_Home_driver m_wpb_home;
static int nLastMotorPos[3];
static bool ad_publish_enable = true;
static std_msgs::Int32MultiArray ad_msg;
static bool input_publish_enable = true;
static std_msgs::Int32MultiArray input_msg;
void CmdVelCallback(const geometry_msgs::Twist::ConstPtr& msg)
{
//ROS_INFO("[wpb_home] liner(%.2f %.2f) angular(%.2f)", msg->linear.x,msg->linear.y,msg->angular.z);
@ -114,6 +119,8 @@ int main(int argc, char** argv)
ros::Subscriber cmd_vel_sub = n.subscribe("cmd_vel",10,&CmdVelCallback);
ros::Subscriber mani_ctrl_sub = n.subscribe("/wpb_home/mani_ctrl",30,&ManiCtrlCallback);
ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu >("imu/data_raw", 100);
ros::Publisher ad_pub = n.advertise<std_msgs::Int32MultiArray>("/wpb_home/ad", 10);
ros::Publisher input_pub = n.advertise<std_msgs::Int32MultiArray>("/wpb_home/input", 10);
ros::NodeHandle n_param("~");
std::string strSerialPort;
@ -319,6 +326,28 @@ int main(int argc, char** argv)
joint_msg.position = joint_pos;
joint_state_pub.publish(joint_msg);
// ad
if(ad_publish_enable == true)
{
ad_msg.data.clear();
for(int i=0;i<15;i++)
{
ad_msg.data.push_back(m_wpb_home.arValAD[i]);
}
ad_pub.publish(ad_msg);
}
//input
if(input_publish_enable == true)
{
input_msg.data.clear();
for(int i=0;i<4;i++)
{
input_msg.data.push_back(m_wpb_home.arValIOInput[i]);
}
input_pub.publish(input_msg);
}
ros::spinOnce();
r.sleep();
}