ROS rtabmap does not create point cloud - ros

I've started to built a stereo camera system to reconstruct the field of view. I use two Logitech C270 webcameras on a base stand to get the image streams.
For the project it's necessary to hold the camera optics as close as I can, so I've turned one camera vertically. I use video_stream_opencv package to get and rotate the images and also to send them to the other nodes.
Because of the further operations and to save some hardware resources, I thought it's necessary to synchronize the images' and camera info's timestamps before calibration, rectification etc., so I've created a synchronization node which uses approximate synchronization between the image frames and camera info messages, and it also republishes the data with the same timestamps.
I thought that after the synchronization won't be necessary to use the approx_sync, but I think I was wrong.
To test the system I also started to use a static tf publisher.
Unfortunately I couldn't get point cloud from the system, but in the terminal a warning message appears frequently:
[ WARN] [1506963490.361523551]: odometry: Could not get transform from base_link to left (stamp=1506963490.228821) after 0.100000 seconds ("wait_for_transform_duration"=0.100000)! Error=". canTransform returned after 0.102307 timeout was 0.1."
Here is my launch file:
<launch>
<!--*******************************************************************************************-->
<!-- Global parameters ************************************************************************-->
<!--*******************************************************************************************-->
<!-- Camera -->
<arg name="fps" default="25" />
<!-- Synchronization -->
<arg name="syncronizer_namespace" default="/syncronizer" />
<arg name="left_camera_raw" default="$(arg syncronizer_namespace)/left" />
<arg name="right_camera_raw" default="$(arg syncronizer_namespace)/right" />
<arg name="left_camera_info_topic" default="$(arg syncronizer_namespace)/left/camera_info" />
<arg name="right_camera_info_topic" default="$(arg syncronizer_namespace)/right/camera_info" />
<!-- Stereo -->
<arg name="stereo_namespace" default="/stereo_camera" />
<arg name="left_image_topic" default="$(arg stereo_namespace)/left/image_rect" />
<arg name="right_image_topic" default="$(arg stereo_namespace)/right/image_rect" />
<arg name="approx_sync" default="true" />
<arg name="queue_size" default="5" />
<!-- Tranfsorm -->
<arg name="use_static_transform" default="true" />
<!-- Visual SLAM -->
<arg name="frame_id" default="base_link" />
<!-- Fixed frame id, set "base_link" or "base_footprint" if they are published -->
<arg name="rtabmap" default="true" />
<arg name="odometry" default="true" />
<!-- Odometry -->
<arg name="odom_frame_id" default="odom" />
<!-- If set, TF is used to get odometry instead of the topic -->
<arg name="ground_truth_frame_id" default="" />
<!-- e.g., "world" -->
<arg name="ground_truth_base_frame_id" default="" />
<!-- e.g., "tracker", a fake frame matching the frame "frame_id" (but on different TF tree) -->
<arg name="wait_for_transform" default="true" />
<arg name="wait_for_transform_duration" default="0.2" />
<!-- 3D visualization -->
<arg name="rviz" default="false" />
<arg name="rtabmapviz" default="true" />
<arg name="camera_info" default="camera_info" />
<!--*******************************************************************************************-->
<!-- Core functionality ***********************************************************************-->
<!--*******************************************************************************************-->
<!-- Camera -->
<group ns="/camera">
<node pkg="nodelet" type="nodelet" name="stereo_camera_nodelet" args="manager" />
<!-- Left video stream input -->
<include file="$(find video_stream_opencv)/launch/camera.launch">
<arg name="camera_name" value="left" />
<arg name="camera_info_url" value="file:///$(find reconstruction)/config/left.yaml" />
<arg name="video_stream_provider" value="1" />
<arg name="flip_horizontal" value="false" />
<arg name="flip_vertical" value="false" />
<arg name="fps" value="$(arg fps)" />
</include>
<!-- Right video stream input -->
<include file="$(find video_stream_opencv)/launch/camera.launch">
<arg name="camera_name" value="right" />
<arg name="camera_info_url" value="file:///$(find reconstruction)/config/right.yaml" />
<arg name="video_stream_provider" value="2" />
<arg name="flip_horizontal" value="false" />
<arg name="flip_vertical" value="true" />
<arg name="fps" value="$(arg fps)" />
</include>
</group>
<!-- Syncronizer -->
<node name="syncronizer" pkg="reconstruction" type="syncronizer" />
<!-- Stereo processing -->
<group ns="/stereo_camera">
<node pkg="nodelet" type="nodelet" name="stereo_nodelet" args="manager" />
<node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc">
<remap from="left/image_raw" to="$(arg left_camera_raw)" />
<remap from="right/image_raw" to="$(arg right_camera_raw)" />
<remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
<remap from="right/camera_info" to="$(arg right_camera_info_topic)" />
<param name="prefilter_size" value="35" />
<param name="prefilter_cap" value="11" />
<param name="correlation_window_size" value="41" />
<param name="min_disparity" value="-15" />
<param name="disparity_range" value="160" />
<param name="uniqueness_ratio" value="0.0" />
<param name="texture_threshold" value="1000" />
<param name="speckle_size" value="500" />
<param name="speckle_range" value="16" />
<param name="approximate_sync" value="true" />
<param name="queue_size" value="5" />
</node>
</group>
<!-- Transform -->
<node if="$(arg use_static_transform)" pkg="tf" type="static_transform_publisher" name="world_to_map" args="0.0 0.0 0.30 0.0 0.0 0.0 /base_link /camera_link 100" />
<group ns="rtabmap">
<!-- Stereo Odometry -->
<node if="$(arg odometry)" pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
<remap from="left/image_rect" to="$(arg left_image_topic)" />
<remap from="right/image_rect" to="$(arg right_image_topic)" />
<remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
<remap from="right/camera_info" to="$(arg right_camera_info_topic)" />
<param name="approx_sync" type="bool" value="$(arg approx_sync)" />
<param name="frame_id" type="string" value="$(arg frame_id)" />
<param name="odom_frame_id" type="string" value="odom" />
<param name="queue_size" type="int" value="5" />
</node>
<!-- Visual SLAM: args: "delete_db_on_start" and "udebug" -->
<node if="$(arg rtabmap)" name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start --udebug">
<remap from="left/image_rect" to="$(arg left_image_topic)" />
<remap from="right/image_rect" to="$(arg right_image_topic)" />
<remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
<remap from="right/camera_info" to="$(arg right_camera_info_topic)" />
<remap from="odom" to="/rtabmap/odom" />
<param name="approx_sync" type="bool" value="$(arg approx_sync)" />
<param name="frame_id" type="string" value="$(arg frame_id)" />
<param name="queue_size" type="int" value="30" />
<param name="subscribe_stereo" type="bool" value="true" />
<param name="subscribe_depth" type="bool" value="false" />
</node>
<!-- Visualisation RTAB-Map -->
<node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
<remap from="left/image_rect" to="$(arg left_image_topic)" />
<remap from="right/image_rect" to="$(arg right_image_topic)" />
<remap from="left/camera_info" to="$(arg left_camera_info_topic)" />
<remap from="right/camera_info" to="$(arg right_camera_info_topic)" />
<remap from="odom_info" to="/rtabmap/odom_info" />
<remap from="odom" to="/rtabmap/odom" />
<param name="frame_id" type="string" value="$(arg frame_id)" />
<param name="queue_size" type="int" value="10" />
<param name="subscribe_stereo" type="bool" value="true" />
<param name="subscribe_odom_info" type="bool" value="true" />
</node>
</group>
</launch>
I also created a rqt graph to understand the connections between the nodes:
rqt_graph
And it's also helpful if we see the tf frames:
tf frames
I hope I said everything to find out what did I wrong, I'm really disapointed because of this problem.

Ok, we found out what was the problem.
As the error is saying, odometry cannot transform frame left to frame base_link. You may want to add a static transform between camera_link and left as this:
<arg name="pi/2" value="1.5707963267948966" />
<node if="$(arg use_static_transform)" pkg="tf" type="static_transform_publisher" name="camera_optical" args="0 0 0 -$(arg pi/2) 0 -$(arg pi/2) /camera_link /left 100" />
... so you have base_link -> camera_link -> left.
Note that you don't need to pre-synchronize as stereo_odometry and rtabmap nodes can do it directly with approx_sync:=true. But yes, if you pre-syncrhonize and set the same timestamp on all image/camera_info topics, you can use approx_sync:=false for odometry and rtabmap. If you have poor results after that, it may be caused by bad stereo rectification and/or synchronization.
I strongly suggest that you get a real stereo camera that does stereo hardware (not software!) synchronization to get good results when the robot is moving.
cheers,
Mathieu

Related

Why is my ORB SLAM 2 launch file crashing?

I am using ROS Melodic on Ubuntu 18.04. I was trying to implement orb slam 2 on a usb camera using this tutorial:https://medium.com/#mhamdaan/implementing-orb-slam-on-ubuntu-18-04-ros-melodic-606e668deffa
I followed all the steps mentioned, but when I launch the roslaunch usb_cam usb_cam-test.launch and roslaunch orb_slam2_ros orb_slam2_p.launch files together in separate terminals, the code crashes.
I get an error:
Enable localization only: false
[orb_slam2_mono-1] process has died [pid 12015, exit code -11, cmd /home/nidhi/tutorials/devel/lib/orb_slam2_ros/orb_slam2_ros_mono /camera/image_raw:=/camera/rgb/image_raw __name:=orb_slam2_mono __log:=/home/nidhi/.ros/log/3be66574-b74a-11ec-8b99-00e18cb0f65d/orb_slam2_mono-1.log].
log file: /home/nidhi/.ros/log/3be66574-b74a-11ec-8b99-00e18cb0f65d/orb_slam2_mono-1*.log
all processes on machine have died, roslaunch will exit
shutting down processing monitor...
... shutting down processing monitor complete
done
I am not able to understand what is going wrong through this error. Also I'll mention that the roslaunch usb_cam usb_cam-test.launch . The launch file I'm using for slam is as follows:
<launch>
<node name="orb_slam2_mono" pkg="orb_slam2_ros"
type="orb_slam2_ros_mono" output="screen">
<remap from="camera/camera_info" to="usb_cam/camera_info" />
<remap from="camera/image_raw" to="usb_cam/image_raw" />
<param name="publish_pointcloud" type="bool" value="true" />
<param name="publish_pose" type="bool" value="true" />
<param name="localize_only" type="bool" value="false" />
<param name="reset_map" type="bool" value="true" />
<!-- static parameters -->
<param name="load_map" type="bool" value="false" />
<param name="map_file" type="string" value="map.bin" />
<param name="voc_file" type="string" value="$(find orb_slam2_ros)/orb_slam2/Vocabulary/ORBvoc.txt" />
<param name="pointcloud_frame_id" type="string" value="map" />
<param name="camera_frame_id" type="string" value="camera_link" />
<param name="min_num_kf_in_map" type="int" value="5" />
<!--ORB parameters-->
<param name="/ORBextractor/nFeatures" type="int" value="2000" />
<param name="/ORBextractor/scaleFactor" type="double" value="1.2" />
<param name="/ORBextractor/nLevels" type="int" value="8" />
<param name="/ORBextractor/iniThFAST" type="int" value="20" />
<param name="/ORBextractor/minThFAST" type="int" value="7" />
<!-- Camera parameters -->
<!-- Camera frames per second -->
<param name="camera_fps" type="int" value="30" />
<!-- Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) -->
<param name="camera_rgb_encoding" type="bool" value="true" />
<!--If the node should wait for a camera_info topic to take the camera calibration data-->
<param name="load_calibration_from_cam" type="bool" value="true" />
</node>
</launch>

redefining global symbol: pi error while ceating a launch file

created a launch file where my robot should open on the gazebo world and when in try to run the launch file using roslaunch i get the following error
redefining global symbol: pi
when processing file: /home/akash/project/src/techman_robot_ros-master/tm_grasp_description/urdf/tm700.urdf.xacro
included from: /home/akash/project/src/techman_robot_ros-master/tm_grasp_description/urdf/tm700_robot.urdf.xacro
xacro.py is deprecated; please use xacro instead
started roslaunch server http://akash:37109/
still i can see my gazebo world but cannot see the robot imported.
and this was the launch file
<launch>
<arg name="eih" default="false" />
<arg name="limited" default="false"/>
<group unless="$(arg eih)">
<param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find tm_grasp_description)/urdf/tm700_robot.urdf.xacro'" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find tm_grasp_description)/urdf/tm700_robot_joint_limited.urdf.xacro'" />
</group>
<group if="$(arg eih)">
<param unless="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find tm_grasp_description)/urdf/tm700_robot_eih.urdf.xacro'" />
<param if="$(arg limited)" name="robot_description" command="$(find xacro)/xacro.py '$(find tm_grasp_description)/urdf/tm700_robot_joint_limited_eih.urdf.xacro'" />
</group>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="paused" value="false"/>
<arg name="use_sim_time" value="true"/>
<arg name="gui" value="true"/>
<arg name="headless" value="false"/>
<arg name="debug" value="false"/>
<arg name="world_name"
value="$(find tm700_moveit_config)/launch/gg.world"/>
</include>
</launch>
Since ROS Jade Xacro internally uses functions and constants from Python math module such as pi when converting a Xacro to a URDF robot description. It seems like tm700.urdf.xacro (and several other Xacro-files in the package) for some reason has pi defined in line 8
<property name="pi" value="3.14159265" />
which is in conflict with the existing definition from the math module. Therefore the generation of your robot description fails. Delete all the definitions of pi from the Xacro-files, then it should work.

Why does my gazebo start in an empty world?

I have written a custom launch file for my custom gazebo world. When I roslaunch the .launch file, only an empty world opens. Could you tell me where am I wrong?
This is my launch file
<launch>
<arg name="x_pos" default="-1.0"/>
<arg name="y_pos" default="-1.0"/>
<arg name="z_pos" default="-1.0"/>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="~/catkin_ws/src/cse340a3_gazebo/worlds/cse340a3.world"/>
<!-- more default parameters can be changed here -->
</include>
<!-- <param name="robot_description" value="~/catkin_ws/src/cse340a3_description/urdf/turtlebot3_burger.urdf.xacro" /> -->
<!-- <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3 -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" /> -->
</launch>
This works for some reason (I put in the full path)
<launch>
<arg name="x_pos" default="-1.0"/>
<arg name="y_pos" default="-1.0"/>
<arg name="z_pos" default="-1.0"/>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<arg name="model" value="burger" doc="model type [burger, waffle, waffle_pi]"/>
<include file="/home/vishwad/catkin_ws/src/cse340a3_gazebo/launch/empty_world.launch">
<arg name="world_name" value="/home/vishwad/catkin_ws/src/cse340a3_gazebo/worlds/cse340a3.world"/>
<!-- more default parameters can be changed here -->
</include>
<!-- <param name="robot_description" value="~/catkin_ws/src/cse340a3_description/urdf/turtlebot3_burger.urdf.xacro" /> -->
<!-- <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model turtlebot3 -x $(arg x_pos) -y $(arg y_pos) -z $(arg z_pos) -param robot_description" /> -->
</launch>

ROS Kinetic, How to pass a parameter from a launch file to the xacro file

I'm trying to pass values from the launch file to the xacro model, so I can, for example, launch two robots in a single world but using different topic names or material colors. Is there a way to do this?
I've tried this unsuccessfully: https://answers.ros.org/question/38956/pass-parameters-to-xacro-in-launch-file/
It gives an error: xxx.launch requires the 'ns' arg to be set.
I also see this here: http://wiki.ros.org/roslaunch/XML/arg But that only shows how to send parameters from one launch file to another.
Here is my launch file:
<launch>
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="gui" default="true"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="ctopic" default="/stupid/test" />
<!-- Load the URDF into the ROS Parameter Server -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find mybot_description)/urdf/mybot.xacro' ctopic:=$(arg ns) "/>
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find mybot_gazebo)/worlds/mybot.world"/>
<arg name="debug" value="$(arg debug)" />
<arg name="gui" value="$(arg gui)" />
<arg name="paused" value="$(arg paused)"/>
<arg name="use_sim_time" value="$(arg use_sim_time)"/>
<arg name="headless" value="$(arg headless)"/>
</include>
<!-- Run a python script to the send a service call to gazebo_ros to spawn a URDF robot -->
<node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
args="-urdf -model mybot -param robot_description"/>
<!-- Run RVIZ
<node name="$(anon rviz)" pkg="rviz" type="rviz" args="$(find mybot_gazebo)/mybot.rviz" output="screen"/>-->
<!-- ros_control mybot launch file -->
<include file="$(find mybot_control)/launch/mybot_control.launch" />
</launch>
I've tried this in my xacro file:
$(arg ctopic)
and
<xacro:property name="ctopic" value="$(arg ctopic)" />
Thanks

How to start and stop jboss server using Ant task?

I need to stop, deploy my ear file and start Jboss server using the Ant tasks.
I am able to compile, build and deploy my J2EE application as an ear file into the JBoss server successfully using Ant tasks. We can see the redeployment of my application in the jboss console. I want to stop the server before deployment and start the server.
Is there any way to do this ?
Here how you start/stop JBoss app container including deploy an application :
<!-- Stop Jboss -->
<target name="stop-jboss" description="Stops back-end EJB container" >
<exec executable="${jboss.bin.dir}/shutdown.bat" spawn="true">
<arg line="-S" />
</exec>
<echo>+-----------------------------+</echo>
<echo>| J B O S S S T O P P E D |</echo>
<echo>+-----------------------------+</echo>
</target>
<!-- Start Jboss -->
<target name="start-jboss" description="Starts back-end EJB container" >
<exec executable="${jboss.bin.dir}/run.bat" spawn="true">
</exec>
<echo>+-----------------------------+</echo>
<echo>| J B O S S S T A R T E D |</echo>
<echo>+-----------------------------+</echo>
</target>
<!-- deploy target-->
<target name="deploy-war" description="deploy war file" depends="prepare">
<sequential>
<antcall target="stop-jboss" />
<war destfile="${file.name}" webxml="conf/web.xml">
<classes dir="bin" />
</war>
<antcall target="start-jboss" />
<echo>+----------------------------+</echo>
<echo>| W A R D E P L O Y E D |</echo>
<echo>+----------------------------+</echo>
</sequential>
</target>
Hope this is helpful :)
Cargo supports ANT and is designed to support several J2EE containers
The appropriate os independent answer would be something like this:
<property name="my.jboss.home" value="/path/to/jboss/install/dir" />
<property name="my.jboss.host" value="localhost" />
<property name="my.jboss.port" value="9999" />
<property name="my.jboss.name" value="my-jboss-instance" />
<property name="my.jboss.debugport" value="8787" />
<!-- supposedly this is built by a seperate task -->
<property name="my.deployment" value="${basedir}/build/deployment.ear" />
<!-- starting preset -->
<presetdef name="start-jboss-preset">
<java jar="${jboss.home}/jboss-modules.jar" fork="true" taskname="${jboss.name}">
<jvmarg value="-server" />
<jvmarg value="-Xms1024m" />
<jvmarg value="-Xmx1024m" />
<jvmarg value="-Dorg.jboss.boot.log.file=${jboss.home}/standalone/log/server.log" />
<jvmarg value="-Dlogging.configuration=file:${jboss.home}/standalone/configuration/logging.properties" />
<arg line="-mp ${jboss.home}/modules/ -jaxpmodule javax.xml.jaxp-provider org.jboss.as.standalone" />
<jvmarg value="-Djboss.home.dir=${jboss.home}" />
<arg value="-b=localhost" />
<arg value="-c=standalone-full.xml" />
<jvmarg value="-Djboss.node.name=${jboss.name}" />
</java>
</presetdef>
<!-- internal task to actually start jboss -->
<target name="start-jboss">
<start-jboss-preset />
</target>
<!-- internal task to start jboss in debug mode -->
<target name="start-jboss-debug">
<start-jboss-preset taskname="dbg:${jboss.name}:${jboss.debugport}">
<jvmarg value="-agentlib:jdwp=transport=dt_socket,address=${jboss.debugport},server=y,suspend=n" />
</start-jboss-preset>
</target>
<!-- preset to run jboss-cli, this can be used to push any command to a running
jboss instance -->
<presetdef name="jboss-cli">
<java jar="${jboss.home}/jboss-modules.jar" fork="true">
<arg line="-mp ${jboss.home}/modules org.jboss.as.cli" />
<arg value="--controller=${jboss.host}:${jboss.port}" />
<arg value="--connect" />
</java>
</presetdef>
<!-- the actual shut down command -->
<target name="exec-jboss">
<jboss-cli failonerror="true">
<arg value="${jboss.command}" />
</jboss-cli>
</target>
<!-- public targets with your properties set -->
<target name="start" description="starts jboss instance">
<antcall target="start-jboss">
<param name="jboss.home" value="${my.jboss.home}" />
<param name="jboss.name" value="${my.jboss.name}" />
</antcall>
</target>
<target name="debug" description="starts jboss instance in debugmode">
<antcall target="start-jboss-debug">
<param name="jboss.home" value="${my.jboss.home}" />
<param name="jboss.name" value="${my.jboss.name}" />
<param name="jboss.debugport" value="${my.jboss.debugport}" />
</antcall>
</target>
<target name="stop" description="stops jboss instance">
<antcall target="exec-jboss">
<param name="jboss.home" value="${my.jboss.home}" />
<param name="jboss.host" value="${my.jboss.host}" />
<param name="jboss.port" value="${my.jboss.port}" />
<param name="jboss.command" value="shutdown" />
</antcall>
</target>
<!-- a dependent build / package task should be present -->
<target name="deploy" description="deploys to a running jboss instance">
<antcall target="exec-jboss">
<param name="jboss.home" value="${my.jboss.home}" />
<param name="jboss.host" value="${my.jboss.host}" />
<param name="jboss.port" value="${my.jboss.port}" />
<param name="jboss.command" value="deploy ${my.deployment}" />
</antcall>
</target>

Resources