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

IMU Enabled

Browse Source
This commit is contained in:
zhangwanjie 2024-08-19 19:42:31 +08:00
parent 857746238d
commit 2382c1a00c
3 changed files with 16 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
wpb_home_bringup/launch/imu_odom.launch
View File

@ -14,7 +14,7 @@
<!-- wpb_home core-->
<node pkg="wpb_home_bringup" type="wpb_home_core" name="wpb_home_core" output="screen">
<param name="serial_port" type="string" value="/dev/ftdi"/>
<param name="imu_odom" type="bool" value="true"/>
<param name="imu" type="bool" value="true"/>
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
</node>

33
wpb_home_bringup/src/wpb_home_core.cpp
View File

@ -152,8 +152,8 @@ int main(int argc, char** argv)
n_param.param<std::string>("serial_port", strSerialPort, "/dev/ttyUSB0");
m_wpb_home.Open(strSerialPort.c_str(),115200);
bool bImuOdom = false;
n_param.param<bool>("imu_odom", bImuOdom, false);
bool bImu = true;
n_param.param<bool>("imu", bImu, true);
bool bOdom = true;
n_param.param<bool>("odom", bOdom, true);
@ -235,7 +235,7 @@ int main(int argc, char** argv)
last_time = current_time;
current_time = ros::Time::now();
if(bImuOdom == false)
if(bOdom == true)
{
double fVx,fVy,fVz;
double fPosDiff[3];
@ -289,12 +289,9 @@ int main(int argc, char** argv)
odom.twist.twist.angular.y = 0;
odom.twist.twist.angular.z = fVz;
if(bOdom == true)
{
//plublishing the odometry and new tf
broadcaster.sendTransform(odom_trans);
odom_pub.publish(odom);
}
//plublishing the odometry and new tf
broadcaster.sendTransform(odom_trans);
odom_pub.publish(odom);
lastVel.linear.x = fVx;
lastVel.linear.y = fVy;
@ -306,21 +303,19 @@ int main(int argc, char** argv)
}
else
{
if(bOdom == true)
{
odom_trans.header.stamp = ros::Time::now();
//plublishing the odometry and new tf
broadcaster.sendTransform(odom_trans);
odom.header.stamp = ros::Time::now();
odom_pub.publish(odom);
//ROS_INFO("[odom] zero");
}
odom_trans.header.stamp = ros::Time::now();
//plublishing the odometry and new tf
broadcaster.sendTransform(odom_trans);
odom.header.stamp = ros::Time::now();
odom_pub.publish(odom);
//ROS_INFO("[odom] zero");
}
pose_diff_pub.publish(pose_diff_msg);
//ROS_INFO("[pose_diff_msg] x= %.2f y=%.2f th= %.2f", pose_diff_msg.x,pose_diff_msg.y,pose_diff_msg.theta);
}
else
if(bImu == true)
{
//imu
sensor_msgs::Imu imu_msg = sensor_msgs::Imu();

2
wpb_home_tutorials/launch/imu_turn.launch
View File

@ -3,7 +3,7 @@
<!-- wpb_home core-->
<node pkg="wpb_home_bringup" type="wpb_home_core" name="wpb_home_core" >
<param name="serial_port" type="string" value="/dev/ftdi"/>
<param name="imu_odom" type="bool" value="true"/>
<param name="imu" type="bool" value="true"/>
<rosparam file="$(find wpb_home_bringup)/config/wpb_home.yaml" command="load" />
</node>