<gazebo reference="camera_link">
<sensor name="camera" type="depth">
- <update_rate>15</update_rate> <!--or 6 or 30 -->
+ <update_rate>90</update_rate> <!--or 6 or 30 -->
<camera>
<horizontal_fov>1.309</horizontal_fov> <!--75 deg at 480p or 87 deg at 720p-->
<image>
- <width>640</width> <!--or 1280 -->
- <height>480</height> <!--or 720 -->
+ <width>1280</width> <!--or 1280 -->
+ <height>720</height> <!--or 720 -->
<format>R8G8B8</format>
</image>
<clip>
<depthImageInfoTopicName>/camera/depth/camera_info</depthImageInfoTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<frameName>plugin_camera_link</frameName>
- <pointCloudCutoff>0.175</pointCloudCutoff> <!-- or 0.28 at 720p -->
+ <pointCloudCutoff>0.28</pointCloudCutoff> <!-- or 0.28 at 720p -->
<pointCloudCutoffMax>10.0</pointCloudCutoffMax>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
- <CxPrime>0</CxPrime>
+ <!-- the color stream has been rectified and {0}'s make sense for the distortion coefficients as such -->
+ <CxPrime>0</CxPrime> <!-- And then other params like cxPrime are simply unused. -->
+ <Fx>0</Fx>
+ <Fy>0</Fy>
<Cx>0</Cx>
<Cy>0</Cy>
- <focalLength>0</focalLength>
+ <focalLength>0</focalLength> <!--The focal length of the D415's left and right imagers is 1.88 mm. The color sensor's focal length is 1.93 mm.-->
<hackBaseline>0</hackBaseline>
</plugin>
</sensor>
<arg name="headless" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="wam" default="false"/>
- <arg name="world" default ="$(find ufrgs_gazebo)/worlds/centenario.world"/>
+ <arg name="world" default ="$(find ufrgs_gazebo)/worlds/centenario_stairs1.world"/>
<arg name="controller" default="twist_mrac_linearizing_controller"/>
<arg name="config" default="$(find twil_bringup)/config/$(arg controller).yaml"/>
- <remap from="/twist_mrac_linearizing_controller/odom" to="/odom" />
+ <remap from="/twist_mrac_linearizing_controller/odom" to="/wheel_odom" />
+
<remap from="/twist_mrac_linearizing_controller/command" to="/cmd_vel" />
-
- <!--<remap from="/camera/color/image_raw" to="/image_color" />-->
- <!--<remap from="/camera/color/camera_info" to= "/my_camera_info"/>-->
- <!--<remap from="/camera/depth/image_raw" to="/image_depth"/>-->
<include file="$(find twil_description)/launch/gazebo.launch" >
<arg name="paused" value="$(arg paused)"/>
<node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 0 0 0 0 0 1 map odom" />
- <!--<node pkg="tf2_ros" type="static_transform_publisher" name="Slam_map_frame_publisher" args="0 0 0 0 0 0 1 map Slam_map" />-->
-
- <!--<node pkg="tf2_ros" type="static_transform_publisher" name="ground_truth_frame_publisher" args="0 0 0 0 0 0 1 map ground_truth" />-->
-
-
-
-
- <!--<node pkg="tf2_ros" type="static_transform_publisher" name="image_color_frame_publisher" args="0 0 0 0 0 0 1 true_odom image_color" />-->
-
- <!--<node pkg="tf2_ros" type="static_transform_publisher" name="camera_info_frame_publisher" args="0 0 0 0 0 0 1 true_odom my_camera_info" />-->
+ <!--<node pkg="tf2_ros" type="static_transform_publisher" name="rgbd_odom_frame_publisher" args="0 0 0 0 0 0 1 map rgbd_odom" />-->
- <!--<node pkg="tf2_ros" type="static_transform_publisher" name="image_depth_frame_publisher" args="0 0 0 0 0 0 1 true_odom image_depth" />-->
-
-
-
-
-
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find twil_location)/rviz/twil_mapping.rviz" required="true"/>
- <node pkg="twil_location" type="twil_location" respawn="false" name="twil_location" output="screen"/>
+ <!--<node pkg="twil_location" type="twil_location" respawn="false" name="twil_location" output="screen"/>-->
<include file="$(find twil_2dnav)/launch/move_base.launch" />
<include file="$(find twil_location)/launch/location.launch" />
-
- <!--<include file="$(find twil_location)/launch/plot.launch" />-->
-
-
</launch>
<launch>
<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen" >
-
+
<remap from="/image" to="/camera/depth/image_raw"/>
- <remap from="/camera_info" to="/camera/depth/camera_info"/>
-
+ <remap from="/camera_info" to="/camera/depth/camera_info"/>
+
<remap from="/scan" to="/amcl_scan"/>
-
+
<param name="output_frame_id" value="camera_link"/>
-
+
+ <param name="scan_height" value="200" />
+ <!--scan_height (int, default: 1 pixel) using 720 pixels, to have the hole high to considered the start of the stairs, cant do this, beacuse it closes the doors with the cealing -->
+
<!--ange_min (double, default: 0.45m)-->
- <!--The minimum ranges to return in meters. Ranges less than this will be output as -Inf. -->
+ <!--The minimum ranges to return in meters. Ranges less than this will be output as -Inf. -->
<!--range_max (double, default: 10.0m)-->
- <!--The maximum ranges to return in meters. Ranges greater than this will be output as +Inf. -->
-
+ <!--The maximum ranges to return in meters. Ranges greater than this will be output as +Inf. -->
+
</node>
-
+
<node pkg="amcl" type="amcl" name="amcl" output="screen" >
-
+
<remap from="/scan" to="/amcl_scan"/>
-
+
<param name="initial_pose_x" value="0"/>
<!--initial_pose_x (double, default: 0.0 meters)-->
<param name="initial_pose_y" value="0"/>
<!--initial_pose_y (double, default: 0.0 meters)-->
<param name="initial_pose_a" value="0"/>
<!--initial_pose_a (double, default: 0.0 radians)-->
- <!--Initial pose mean (x,Y,yaw), used to initialize filter with Gaussian distribution. -->
+ <!--Initial pose mean (x,Y,yaw), used to initialize filter with Gaussian distribution. -->
<param name="initial_cov_xx" value="0.25"/>
<!--initial_cov_xx (double, default: 0.5*0.5 meters)-->
<param name="initial_cov_yy" value="0.25"/>
<!--initial_cov_yy (double, default: 0.5*0.5 meters)-->
- <!--itial_cov_aa (double, default: (π/12)*(π/12) radian)-->
+ <!--itial_cov_aa (double, default: (π/12)*(π/12) radian)-->
- <!--Initial pose covariance (x*x,y*y,yaw*yaw), used to initialize filter with Gaussian distribution.-->
-
- <param name="use_map_topic" value="true"/>
- <!--~use_map_topic (bool, default: false)-->
- <!--When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.-->
-
- <param name="base_frame_id " value="twil_origin"/>
-
- <param name="laser_min_range" value="0.45"/>
- <!--laser_min_range (double, default: -1.0)-->
- <!--Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used. -->
+ <!--Initial pose covariance (x*x,y*y,yaw*yaw), used to initialize filter with Gaussian distribution.-->
+
+ <param name="use_map_topic" value="true"/>
+ <!--~use_map_topic (bool, default: false)-->
+ <!--When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.-->
+
+ <param name="base_frame_id " value="twil_origin"/>
+
+ <param name="laser_min_range" value="0.45"/>
+ <!--laser_min_range (double, default: -1.0)-->
+ <!--Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used. -->
<param name="laser_max_range" value="10.0"/>
<!--laser_max_range (double, default: -1.0)-->
- <!--Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used. -->
-
- <param name="min_particles" value="1000.0"/>
-
- <param name="max_particles" value="5000.0"/>
+ <!--Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used. -->
+
+ <param name="min_particles" value="1000.0"/>
+
+ <param name="max_particles" value="5000.0"/>
- <param name="odom_model_type" value="diff"/>
- <!-- If ~odom_model_type is "diff" then we use the sample_motion_model_odometry algorithm from Probabilistic Robotics, p136; this model uses the noise parameters odom_alpha1 through odom_alpha4, as defined in the book. -->
-
+ <param name="odom_model_type" value="diff-corrected"/>
+ <!-- If ~odom_model_type is "diff" then we use the sample_motion_model_odometry algorithm from Probabilistic Robotics, p136; this model uses the noise parameters odom_alpha1 through odom_alpha4, as defined in the book. -->
+
+ <param name="update_min_d" value="0.2"/>
+
+ <param name="update_min_a" value="0.1"/>
+
+ <param name="odom_alpha1" value="0.405"/>
<!--odom_alpha1 (double, default: 0.2)-->
- <!--Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.-->
+ <!--Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.-->
+ <param name="odom_alpha2" value="0.005"/>
<!--odom_alpha2 (double, default: 0.2)-->
- <!--Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion.-->
+ <!--Specifies the expected noise in odometry's rotation estimate from translational component of the robot's motion.-->
+ <param name="odom_alpha3" value="0.011"/>
<!--odom_alpha3 (double, default: 0.2)-->
- <!--pecifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.-->
+ <!--pecifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.-->
+ <param name="odom_alpha4" value="0.0025"/>
<!--odom_alpha4 (double, default: 0.2)-->
- <!--Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.-->
-
- </node>
-
-
-
-
- <!--<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" >-->
- <!--<remap from="/rgb/image" to="/camera/color/image_raw" />-->
- <!--<remap from="/rgb/camera_info" to= "/camera/depth/camera_info" />-->
- <!--<remap from="/depth/image" to="/camera/depth/image_raw" />-->
-
- <!--<remap from="/odom" to="/rgbd_odom" />-->
-
- <!--<param name="frame_id" value="twil_origin" />-->
- <!--<param name="Odom/Strategy" value="0" />-->
- <!--Odom/Strategy = "0"[0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion]-->
-
- <!--<param name="Odom/Holonomic" value="false"/>-->
- <!--<param name="OdomF2M/BundleAdjustment" value="2" />-->
- <!--<param name="Odom/FilteringStrategy" value="0" />-->
-
-
- <!--</node>-->
-
- <!--<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">-->
- <!--<param name="output_frame" value="odom"/>-->
- <!--<param name="freq" value="30.0"/>-->
-
- <!--<param name="sensor_timeout" value="1.0"/>-->
-
- <!--<param name="odom_used" value="true"/>-->
- <!--<param name="imu_used" value="false"/>-->
- <!--<param name="vo_used" value="true"/>-->
-
- <!--<param name="debug" value="false"/>-->
- <!--<param name="self_diagnose" value="false"/>-->
-
- <!--<remap from="/vo" to="/rgbd_odom" />-->
-
- <!--</node>-->
-
+ <!--Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.-->
+
+
+ <param name="laser_z_hit" value="0.99"/>
+ <param name="laser_z_max" value="0.0"/>
+ <param name="laser_z_rand" value="0.01"/>
+
+
+ <param name="kld_err" value="0.001"/>
+ <param name="kld_z" value="0.995"/>
+
+ </node>
+
+
+
+ <!-- <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+ <param name="output_frame" value="odom"/>
+ <param name="freq" value="30.0"/>
+
+ <param name="sensor_timeout" value="1.0"/>
+
+ <param name="odom_used" value="true"/>
+ <param name="imu_used" value="false"/>
+ <param name="vo_used" value="true"/>
+
+ <param name="debug" value="false"/>
+ <param name="self_diagnose" value="false"/>
+
+ <remap from="/vo" to="/rgbd_odom" />
+ <remap from="/odom" to="/wheel_odom" />
+
+ </node>-->
+
+
+ <!--<node pkg="rtabmap_ros" type="rgbd_odometry" name="visual_odometry" output="screen" >
+ <remap from="/rgb/image" to="/camera/color/image_raw" />
+ <remap from="/rgb/camera_info" to= "/camera/depth/camera_info" />
+ <remap from="/depth/image" to="/camera/depth/image_raw" />
+
+ <remap from="/odom" to="/rgbd_odom" />
+
+ <param name="odom_frame_id" value="rgbd_odom" />
+
+ <param name="frame_id" value="twil_origin" />
+ <param name="Odom/Strategy" value="1" />-->
+ <!--Odom/Strategy = "0"[0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion]-->
+
+ <!--<param name="Odom/Holonomic" value="false"/>
+ <param name="OdomF2M/BundleAdjustment" value="2" />
+ <param name="Odom/FilteringStrategy" value="0" />
+
+
+ </node>-->
+
+
+<!-- <node pkg="rtabmap_ros" type="rtabmap" name="rtabmap_ros" output="screen">
+ <remap from="grid_map" to="map" />
+
+ <remap from="/odom" to="/wheel_odom" />
+
+ <remap from="/rgb/image" to="/camera/color/image_raw" />
+ <remap from="/rgb/camera_info" to= "/camera/depth/camera_info" />
+ <remap from="/depth/image" to="/camera/depth/image_raw" />
+
+ <remap from="/scan" to="/amcl_scan" />
+
+ <param name="frame_id" value="twil_origin" />
+ <param name="odom_frame_id" value="odom" />
+ <param name="map_frame_id" value="map" />
+
+ <param name="queue_size" value="50"/>
+
+ <param name="approx_sync" value="true"/>
+
+ <param name="subscribe_scan" value="false" />
+
+ <param name="publish_tf" value="false"/>
+
+ </node>-->
+
</launch>
+++ /dev/null
-<launch>
-
- <arg name="paused" default="true"/>
- <arg name="headless" default="false"/>
- <arg name="use_sim_time" default="true"/>
- <arg name="wam" default="false"/>
- <arg name="world" default ="$(find ufrgs_gazebo)/worlds/centenario.world"/>
-
- <include file="$(find twil_description)/launch/gazebo.launch" >
- <arg name="paused" value="$(arg paused)"/>
- <arg name="headless" value="$(arg headless)"/>
- <arg name="use_sim_time" value="$(arg use_sim_time)"/>
- <arg name="wam" value="$(arg wam)"/>
- <arg name="d435" value="true"/>
- <arg name="world" value="$(arg world)" />
- </include>
-
-</launch>
+++ /dev/null
-<launch>
-
- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_x" output="screen" args = "/odom/pose/pose/position/x /rgbd_odom/pose/pose/position/x /ground_truth/pose/pose/position/x /robot_pose_ekf/odom_combined/pose/pose/position/x"/>
-
- <node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_y" output="screen" args = "/odom/pose/pose/position/y /rgbd_odom/pose/pose/position/y /ground_truth/pose/pose/position/y /robot_pose_ekf/odom_combined/pose/pose/position/y" />
-
- <!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_x" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/x /rgbd_odom/pose/pose/orientation/x /true_odom/pose/pose/orientation/x /robot_pose_ekf/odom_combined/pose/pose/orientation/x" />-->
-
- <!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_y" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/y /rgbd_odom/pose/pose/orientation/y /true_odom/pose/pose/orientation/y /robot_pose_ekf/odom_combined/pose/pose/orientation/y" />-->
-
- <!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_z" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/z /rgbd_odom/pose/pose/orientation/z /true_odom/pose/pose/orientation/z /robot_pose_ekf/odom_combined/pose/pose/orientation/z" />-->
-
- <!--<node pkg="rqt_plot" type="rqt_plot" name="rqt_plot_ori_w" output="screen" args = "/twist_mrac_linearizing_controller/odom/pose/pose/orientation/w /rgbd_odom/pose/pose/orientation/w /true_odom/pose/pose/orientation/w /robot_pose_ekf/odom_combined/pose/pose/orientation/w" />-->
-
-</launch>
Property Tree Widget:
Expanded:
- /DepthCloud1/Auto Size1
+ - /Odometry2/Shape1
Splitter Ratio: 0.5
- Tree Height: 370
+ Tree Height: 299
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Experimental: true
Name: Time
SyncMode: 0
- SyncSource: DepthCloud
+ SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
Topic: /twist_mrac_linearizing_controller/odom
Unreliable: false
Value: true
- - Alpha: 0.699999988079071
-
- Class: rviz/Odometry
- Covariance:
- Orientation:
- Alpha: 0.5
- Color: 255; 255; 127
- Color Style: Unique
- Frame: Local
- Offset: 1
- Scale: 1
- Value: true
- Position:
- Alpha: 0.30000001192092896
- Color: 204; 51; 204
- Scale: 1
- Value: true
- Value: true
- Enabled: true
- Keep: 42
- Name: rgbd_Odometry
- Position Tolerance: 0.30000001192092896
- Queue Size: 10
- Shape:
- Alpha: 1
- Axes Length: 1
- Axes Radius: 0.10000000149011612
- Color: 25; 255; 0
- Head Length: 0.30000001192092896
- Head Radius: 0.10000000149011612
- Shaft Length: 1
- Shaft Radius: 0.05000000074505806
- Value: Arrow
- Topic: /rgbd_odom
- Unreliable: false
- Value: true
- - Alpha: 0.699999988079071
-
+ - Angle Tolerance: 0.10000000149011612
Class: rviz/Odometry
Covariance:
Orientation:
Unreliable: false
Value: true
- Alpha: 0.699999988079071
-
-
-
-
Class: rviz/Map
Color Scheme: costmap
Draw Behind: false
Value: true
- Alpha: 1
Axes Length: 1
- Axes Radius: 0.10000000149011612
-
- Class: rviz/Map
- Color Scheme: map
- Draw Behind: false
- Enabled: true
- Name: Map(SLAM)
- Topic: /Slam_map
- Unreliable: false
- Use Timestamp: false
- Value: true
- - Alpha: 1
- Axes Length: 1
- Axes Radius: 0.10000000149011612
-
+ Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 255; 0
Enabled: true
Head Length: 0.30000001192092896
- Head Radius: 0.10000000149011612
+ Head Radius: 0.10000000149011612
Name: Pose (goal)
Queue Size: 10
Shaft Length: 1
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
- Max Intensity: 4096
Min Color: 0; 0; 0
- Min Intensity: 0
Name: DepthCloud
Occlusion Compensation:
Occlusion Time-Out: 30
Odometry: true
Path: true
Pose (goal): true
+ PoseWithCovariance: true
RobotModel: true
Value: true
+ ground_truth: true
Zoom Factor: 1
- Class: rviz/Camera
Enabled: true
Odometry: true
Path: true
Pose (goal): true
+ PoseWithCovariance: true
RobotModel: true
Value: true
+ ground_truth: true
Zoom Factor: 1
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 25; 255; 25
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: PoseWithCovariance
+ Queue Size: 10
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /amcl_pose
+ Unreliable: false
+ Value: true
+ - Angle Tolerance: 0.10000000149011612
+ Class: rviz/Odometry
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Keep: 100
+ Name: Odometry
+ Position Tolerance: 0.10000000149011612
+ Queue Size: 10
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 255; 25
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /rgbd_odom
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/PoseWithCovariance
+ Color: 255; 25; 255
+ Covariance:
+ Orientation:
+ Alpha: 0.5
+ Color: 255; 255; 127
+ Color Style: Unique
+ Frame: Local
+ Offset: 1
+ Scale: 1
+ Value: true
+ Position:
+ Alpha: 0.30000001192092896
+ Color: 204; 51; 204
+ Scale: 1
+ Value: true
+ Value: true
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: PoseWithCovariance
+ Queue Size: 10
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /robot_pose_ekf/odom_combined
+ Unreliable: false
+ Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
collapsed: false
Displays:
collapsed: false
- Height: 1025
+ Height: 911
Hide Left Dock: false
Hide Right Dock: true
- QMainWindow State: 000000ff00000000fd00000004000000000000015600000367fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001fb000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c00430061006d00650072006100200028006400650070007400680029010000023c000000980000001600fffffffb0000001c00430061006d006500720061002000280063006f006c006f0072002901000002da000000c80000001600ffffff000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002c400fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ QMainWindow State: 000000ff00000000fd0000000400000000000001fd000002f1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c00430061006d0065007200610020002800640065007000740068002901000001f9000000830000001600fffffffb0000001c00430061006d006500720061002000280063006f006c006f007200290100000282000000ac0000001600ffffff000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000031b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000535000002f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Views:
collapsed: true
- Width: 1853
- X: 59
- Y: 4
+ Width: 1848
+ X: 72
+ Y: 27
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/LaserScan.h>
#include <nav_msgs/Odometry.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <kdl_parser/kdl_parser.hpp>
location(ros::NodeHandle node);
~location(void);
void dataFusion(void);
-
+
private:
ros::NodeHandle node_;
-
+
ros::Subscriber visualodometrySubscriber;
- ros::Subscriber odometrySubscriber;
+ ros::Subscriber odometrySubscriber;
+ ros::Subscriber efkPoseSubscriber;
ros::Publisher fusedOdomPublisher;
const int nJoints;
Eigen::MatrixXd P2;
ros::Time lastTime;
-
+
std::vector<double> wheelRadius;
double wheelBase;
-
- nav_msgs::Odometry _odometry;
+
+ nav_msgs::Odometry odom;
nav_msgs::Odometry _rgbd_odometry;
-
+ geometry_msgs::PoseWithCovarianceStamped _ekf_pose;
+
void VisualOdomCB(const nav_msgs::Odometry::ConstPtr &rgbd_odometry);
- void OdomCB(const nav_msgs::Odometry::ConstPtr &odometry);
+ void OdomCB(const nav_msgs::Odometry::ConstPtr &wheel_odom);
+ void ekfCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &ekf_pose);
};
location::location(ros::NodeHandle node):
- nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),wheelRadius(nJoints),_odometry()
+ nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),wheelRadius(nJoints),odom()
{
node_=node;
-
+
visualodometrySubscriber= node_.subscribe("rgbd_odom",1000,&location::VisualOdomCB,this);
- odometrySubscriber = node_.subscribe("odom",1000,&location::OdomCB,this);
-
- fusedOdomPublisher=node_.advertise<nav_msgs::Odometry>("odom_fused",1000);
-
+ odometrySubscriber = node_.subscribe("wheel_odom",1000,&location::OdomCB,this);
+ efkPoseSubscriber = node_.subscribe("robot_pose_ekf/odom_combined", 1000,&location::ekfCB,this);
+
+ fusedOdomPublisher=node_.advertise<nav_msgs::Odometry>("odom",1000);
+
lastTime=ros::Time::now();
-
}
location::~location(void)
{
visualodometrySubscriber.shutdown();
odometrySubscriber.shutdown();
+ efkPoseSubscriber.shutdown();
}
-void location::OdomCB(const nav_msgs::Odometry::ConstPtr &odometry)
+void location::OdomCB(const nav_msgs::Odometry::ConstPtr &wheel_odom)
{
- _odometry.header.seq = odometry->header.seq;
- _odometry.header.stamp = odometry->header.stamp;
- _odometry.header.frame_id = odometry->header.frame_id;
-
- _odometry.pose.pose.position.x = odometry->pose.pose.position.x;
- _odometry.pose.pose.position.y = odometry->pose.pose.position.y;
- _odometry.pose.pose.position.z = odometry->pose.pose.position.z;
-
- _odometry.pose.pose.orientation.x = odometry->pose.pose.orientation.x;
- _odometry.pose.pose.orientation.y = odometry->pose.pose.orientation.y;
- _odometry.pose.pose.orientation.z = odometry->pose.pose.orientation.z;
- _odometry.pose.pose.orientation.w = odometry->pose.pose.orientation.w;
-
- _odometry.pose.covariance = odometry->pose.covariance;
-
- _odometry.twist.twist.linear.x = odometry->twist.twist.linear.x;
- _odometry.twist.twist.linear.y = odometry->twist.twist.linear.y;
- _odometry.twist.twist.linear.z = odometry->twist.twist.linear.z;
-
- _odometry.twist.twist.angular.x = odometry->twist.twist.angular.x;
- _odometry.twist.twist.angular.y = odometry->twist.twist.angular.y;
- _odometry.twist.twist.angular.z = odometry->twist.twist.angular.z;
-
- _odometry.twist.covariance = odometry->twist.covariance;
+ odom.header.seq = wheel_odom->header.seq;
+ odom.header.stamp = wheel_odom->header.stamp;
+ odom.header.frame_id = wheel_odom->header.frame_id;
+
+ odom.child_frame_id = wheel_odom->child_frame_id;
+
+ odom.pose.pose.position.x = wheel_odom->pose.pose.position.x;
+ odom.pose.pose.position.y = wheel_odom->pose.pose.position.y;
+ odom.pose.pose.position.z = wheel_odom->pose.pose.position.z;
+
+ odom.pose.pose.orientation.x = wheel_odom->pose.pose.orientation.x;
+ odom.pose.pose.orientation.y = wheel_odom->pose.pose.orientation.y;
+ odom.pose.pose.orientation.z = wheel_odom->pose.pose.orientation.z;
+ odom.pose.pose.orientation.w = wheel_odom->pose.pose.orientation.w;
+
+ odom.pose.covariance = wheel_odom->pose.covariance;
+
+ odom.twist.twist.linear.x = wheel_odom->twist.twist.linear.x;
+ odom.twist.twist.linear.y = wheel_odom->twist.twist.linear.y;
+ odom.twist.twist.linear.z = wheel_odom->twist.twist.linear.z;
+
+ odom.twist.twist.angular.x = wheel_odom->twist.twist.angular.x;
+ odom.twist.twist.angular.y = wheel_odom->twist.twist.angular.y;
+ odom.twist.twist.angular.z = wheel_odom->twist.twist.angular.z;
+
+ odom.twist.covariance = wheel_odom->twist.covariance;
+}
+
+void location::ekfCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &ekf_pose)
+{
+ _ekf_pose.header.seq = ekf_pose->header.seq;
+ _ekf_pose.header.stamp = ekf_pose->header.stamp;
+ _ekf_pose.header.frame_id = ekf_pose->header.frame_id;
+
+ _ekf_pose.pose = ekf_pose->pose;
}
void location::VisualOdomCB(const nav_msgs::Odometry::ConstPtr &rgbd_odometry)
_rgbd_odometry.header.seq = rgbd_odometry->header.seq;
_rgbd_odometry.header.stamp = rgbd_odometry->header.stamp;
_rgbd_odometry.header.frame_id = rgbd_odometry->header.frame_id;
-
+
+ _rgbd_odometry.child_frame_id = rgbd_odometry->child_frame_id;
+
_rgbd_odometry.pose.pose.position.x = rgbd_odometry->pose.pose.position.x;
_rgbd_odometry.pose.pose.position.y = rgbd_odometry->pose.pose.position.y;
_rgbd_odometry.pose.pose.position.z = rgbd_odometry->pose.pose.position.z;
-
+
_rgbd_odometry.pose.pose.orientation.x = rgbd_odometry->pose.pose.orientation.x;
_rgbd_odometry.pose.pose.orientation.y = rgbd_odometry->pose.pose.orientation.y;
_rgbd_odometry.pose.pose.orientation.z = rgbd_odometry->pose.pose.orientation.z;
_rgbd_odometry.pose.pose.orientation.w = rgbd_odometry->pose.pose.orientation.w;
-
+
_rgbd_odometry.pose.covariance = rgbd_odometry->pose.covariance;
-
+
_rgbd_odometry.twist.twist.linear.x = rgbd_odometry->twist.twist.linear.x;
_rgbd_odometry.twist.twist.linear.y = rgbd_odometry->twist.twist.linear.y;
_rgbd_odometry.twist.twist.linear.z = rgbd_odometry->twist.twist.linear.z;
-
+
_rgbd_odometry.twist.twist.angular.x = rgbd_odometry->twist.twist.angular.x;
_rgbd_odometry.twist.twist.angular.y = rgbd_odometry->twist.twist.angular.y;
_rgbd_odometry.twist.twist.angular.z = rgbd_odometry->twist.twist.angular.z;
-
+
_rgbd_odometry.twist.covariance = rgbd_odometry->twist.covariance;
}
void location::dataFusion()
{
- fusedOdomPublisher.publish(_odometry);
+ odom.pose = _ekf_pose.pose;
+
+ fusedOdomPublisher.publish(odom);
}
{
ros::init(argc,argv,"twil_location");
ros::NodeHandle node;
-
+
location location(node);
ros::Rate loop(100);
while(ros::ok())
{
+
location.dataFusion();
-
+
ros::spinOnce();
loop.sleep();
}
projects / twil.git / commitdiff