Posted by
Drane on
URL: http://official-rtab-map-forum.206.s1.nabble.com/Autonomous-Navigation-using-RTAB-Map-s-RGBD-Odometry-tp6223.html
Hi Mr Mathieu,
Is it possible to do autonomous navigation using RTAB-Map's RGBD Odometry as the only odometry source?
I've tried to do this using the following commands:
Mapping
roscore
rosrun serial serial
rosrun rosserial_python serial_node.py /dev/ttyUSB0 _baud:=230400
roslaunch kinect2_bridge kinect2_bridge.launch publish_tf:=true
roslaunch rtabmap_ros rgbd_mapping_kinect2.launch resolution:=qhd rtabmap_args:="--delete_db_on_start" rviz:=true rtabmapviz:=false
rosrun teleop_twist_keyboard teleop_twist_keyboard.py
rosrun depthimage_to_laserscan depthimage_to_laserscan image:=/kinect2/qhd/image_depth_rect camera_info:=/camera/camera_info
My robot doesn't have nav_msgs/Odometry and tf, then I'm trying to use the /rtabmap/odom topic:
publish_odom.cpp
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include "geometry_msgs/Pose2D.h"
#include "geometry_msgs/Twist.h"
#include "std_msgs/Char.h"
#include "std_msgs/UInt16.h"
#include "angles/angles.h"
#include "iostream"
ros::Publisher pub_velocity;
ros::Timer tim_50hz;
//odometry & gyro
double pos_x,pos_y;
double theta;
double buffer_pos_x, buffer_pos_y, buffer_theta;
double offset_pos_x = 0, offset_pos_y = 0, offset_theta = 0;
double linearposx = 0, linearposy = 0, quatx = 0, quaty = 0, quatz = 0, quatw = 0, vex = 0, vey = 0, vez = 0;
//velocity
double vx = 0,vy = 0,vw = 0;
double buffer_vx = 0, buffer_vy = 0, buffer_vw = 0;
double buffer_vx_new = 0, buffer_vy_new = 0, buffer_vw_new = 0;
void manual_move(int _vx, int _vy, int _vw)
{
geometry_msgs::Twist msg_velocity;
msg_velocity.linear.x = vx = _vx;
msg_velocity.linear.y = vy = _vy;
msg_velocity.angular.z = vw = _vw;
pub_velocity.publish(msg_velocity);
}
void callback_velocity (const geometry_msgs::TwistPtr &msg)
{
buffer_vx_new = msg->linear.x;
buffer_vx = buffer_vx_new *40;
buffer_vy_new = msg->linear.y;
buffer_vy = buffer_vy_new *40;
buffer_vw_new = msg->angular.z;
buffer_vw = buffer_vw_new *40;
}
void callback_odometry (const nav_msgs::Odometry::ConstPtr &msg)
{
linearposx=msg->pose.pose.position.x;
linearposy=msg->pose.pose.position.y;
quatx=msg->pose.pose.orientation.x;
quaty=msg->pose.pose.orientation.y;
quatz=msg->pose.pose.orientation.z;
quatw=msg->pose.pose.orientation.w;
vex=msg->twist.twist.linear.x;
vey=msg->twist.twist.linear.y;
vez=msg->twist.twist.angular.z;
}
void callback_50hz(const ros::TimerEvent &time)
{
manual_move(buffer_vx,buffer_vy,buffer_vw);
ROS_INFO("%f", theta);
}
int main(int argc, char** argv){
ros::init(argc, argv, "odometry_publisher");
ros::NodeHandle n;
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
pub_velocity = n.advertise<geometry_msgs::Twist>("pc2stm_velocity",8);
tf::TransformBroadcaster odom_broadcaster;
ros::Subscriber sub_odometry = n.subscribe("/rtabmap/odom", 50,callback_odometry);
ros::Subscriber sub_velocity = n.subscribe("cmd_vel", 8, callback_velocity);
tim_50hz = n.createTimer(ros::Duration(0.02), callback_50hz);
ros::Time current_time, last_time;
current_time = ros::Time::now();
last_time = ros::Time::now();
ros::Rate r(50);
while(n.ok()){
ros::spinOnce(); // check for incoming messages
current_time = ros::Time::now();
//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_link";
odom_trans.transform.translation.x = linearposx;
odom_trans.transform.translation.y = linearposy;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation.x = quatx;
odom_trans.transform.rotation.y = quaty;
odom_trans.transform.rotation.z = quatz;
odom_trans.transform.rotation.w = quatw;
//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 = linearposx;
odom.pose.pose.position.y = linearposy;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation.x = quatx;
odom.pose.pose.orientation.y = quaty;
odom.pose.pose.orientation.z = quatz;
odom.pose.pose.orientation.w = quatw;
//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vex;
odom.twist.twist.linear.y = vey;
odom.twist.twist.angular.z = vez;
//publish the message
odom_pub.publish(odom);
last_time = current_time;
r.sleep();
}
}
tf_broadcaster.cpp
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc, char** argv){
ros::init(argc, argv, "mobile_robot_tf_publisher");
ros::NodeHandle nh;
ros::Rate r(100);
tf::TransformBroadcaster base_to_laser_broadcaster;
while(nh.ok()){
/*base_to_laser_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
ros::Time::now(),"map", "odom"));*/
base_to_laser_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
ros::Time::now(),"odom", "base_link"));
base_to_laser_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0.07, 0.0, 0.64)),
ros::Time::now(),"base_link", "base_laser"));
base_to_laser_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
ros::Time::now(),"base_laser", "camera_link"));
base_to_laser_broadcaster.sendTransform(
tf::StampedTransform(
tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
ros::Time::now(),"camera_link", "kinect2_base_link"));
r.sleep();
}
}
<?xml version="1.0"?>
<launch>
<master auto="start"/>
<arg name="map_topic" default="/rtabmap/grid_map"/>
<remap from="map" to="$(arg map_topic)"/>
<!-- Run the map server
<node name="map_server" pkg="map_server" type="map_server" args="$(find mobile_robot_2dnav)/maps/map.yaml"/> -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
<rosparam file="$(find mobile_robot_2dnav)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
<rosparam file="$(find mobile_robot_2dnav)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
<rosparam file="$(find mobile_robot_2dnav)/param/local_costmap_params.yaml" command="load" />
<rosparam file="$(find mobile_robot_2dnav)/param/global_costmap_params.yaml" command="load" />
<rosparam file="$(find mobile_robot_2dnav)/param/base_local_planner_params.yaml" command="load" />
</node>
</launch>
When I do the mapping process the robot's tf moved according to the robot, but when I run the publish_odom.cpp the robot's tf is not moving correctly, and there are errors relating to the odom's quaternion. I can't set the 2D Pose Estimate in rviz too and when I send the 2D Nav Goal the robot will travel some distance and then keep rotating in place.
So I think the odom is not published correctly or the subscribed topic is wrong? How can I publish RTAB-Map's RGBD Odometry correctly?
Thank You