--- /dev/null
+cmake_minimum_required(VERSION 3.0.2)
+project(twil_location)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ kdl_parser
+ roscpp
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# std_msgs # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES twil_location
+# CATKIN_DEPENDS roscpp
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+ include
+ ${catkin_INCLUDE_DIRS}
+ ${EIGEN3_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/twil_location.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+ add_executable(${PROJECT_NAME} src/twil_location.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+ target_link_libraries(${PROJECT_NAME}
+ ${catkin_LIBRARIES}
+ )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_twil_location.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
--- /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"/>
+ <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/command" to="/cmd_vel" />
+
+ <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>
+
+ <rosparam file="$(arg config)" command="load"/>
+
+ <include file="$(find twil_bringup)/launch/$(arg controller).launch" />
+
+ <node pkg="tf2_ros" type="static_transform_publisher" name="odom_frame_publisher" args="0 0 0 0 0 0 1 map odom" />
+
+ <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"/>
+
+ <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>
--- /dev/null
+<launch>
+ <!--<node pkg="hector_mapping" type="hector_mapping" name="slam_mapping" output="screen" >-->
+ <!--<remap from="scan" to="/slam_scan" />-->
+ <!--<param name="odom_frame" value="/odom_frame_publisher" />-->
+ <!--<param name="base_frame" value="/robot_state_publisher/twil_origin" />-->
+ <!--<remap from="map" to="Slam_map" />-->
+ <!--</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/color/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" />
+ </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>
+
+ <!--<node name="rtabmap" pkg="rtabmap_ros" type="rtabmap">-->
+ <!--<param name="Optimizer/Iterations" type="int" value="10"/>-->
+
+ <!--<remap from="/odom" to="/rgbd_odom" />-->
+
+ <!--<param name="subscribe_depth" type="bool" value="false"/>-->
+ <!--<param name="subscribe_rgbd" type="bool" value="true"/>-->
+ <!--<param name="subscribe_scan" type="bool" value="false"/>-->
+
+ <!--<remap from="/rgb/image" to="/camera/color/image_raw" />-->
+ <!--<remap from="/rgb/camera_info" to= "/camera/color/camera_info" />-->
+ <!--<remap from="/depth/image" to="/camera/depth/image_raw" />-->
+
+ <!--<remap from="grid_map" to="Slam_map" />-->
+
+ <!--<param name="frame_id" value="twil_origin" />-->
+ <!--<param name="queue_size" value="50" />-->
+
+ <!--<param name="odom_frame_id" value="slam_odom" />-->
+
+ <!--<param name="approx_sync" value="false" />-->
+ <!--<param name="queue_size" value="20" />-->
+ <!--<param name="map_always_update" value="true" />-->
+ <!--<param name="wait_for_transform_duration" value="1" />-->
+
+ <!--</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 /true_odom/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 /true_odom/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 = "/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 = "/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 = "/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 = "/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>
--- /dev/null
+<?xml version="1.0"?>
+<package format="2">
+ <name>twil_location</name>
+ <version>0.0.0</version>
+ <description>The twil_location package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="raul@todo.todo">raul</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but multiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_location</url> -->
+
+
+ <!-- Author tags are optional, multiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintainers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+ <!-- The *depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+ <!-- <depend>roscpp</depend> -->
+ <!-- Note that this is equivalent to the following: -->
+ <!-- <build_depend>roscpp</build_depend> -->
+ <!-- <exec_depend>roscpp</exec_depend> -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use build_export_depend for packages you need in order to build against this package: -->
+ <!-- <build_export_depend>message_generation</build_export_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use exec_depend for packages you need at runtime: -->
+ <!-- <exec_depend>message_runtime</exec_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <!-- Use doc_depend for packages you need only for building documentation: -->
+ <!-- <doc_depend>doxygen</doc_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>roscpp</build_depend>
+ <build_export_depend>roscpp</build_export_depend>
+ <exec_depend>roscpp</exec_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+Panels:
+ - Class: rviz/Displays
+ Help Height: 78
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /DepthCloud1/Auto Size1
+ Splitter Ratio: 0.5
+ Tree Height: 370
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Experimental: true
+ Name: Time
+ SyncMode: 0
+ SyncSource: DepthCloud
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Alpha: 0.5
+ Cell Size: 1
+ Class: rviz/Grid
+ Color: 160; 160; 164
+ Enabled: true
+ Line Style:
+ Line Width: 0.029999999329447746
+ Value: Lines
+ Name: Grid
+ Normal Cell Count: 0
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Plane: XY
+ Plane Cell Count: 50
+ Reference Frame: <Fixed Frame>
+ Value: true
+ - Alpha: 1
+ Class: rviz/RobotModel
+ Collision Enabled: false
+ Enabled: true
+ Links:
+ All Links Enabled: true
+ Expand Joint Details: false
+ Expand Link Details: false
+ Expand Tree: false
+ Link Tree Style: Links in Alphabetic Order
+ battery:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bottom_sonar_box:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bottom_sonar_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ bottom_sonar_sensor:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera_bottom_screw_frame:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ camera_usb_plug_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ caster_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ chassis:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ chassis_top:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ cpu:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ fan:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_caster_base:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_caster_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ front_caster_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ left_wheel_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ motor_driver:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ plugin_camera_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ power_supply:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_wheel:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ right_wheel_support:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ stepper_driver:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ top_sonar_box:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ top_sonar_cover:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ top_sonar_sensor:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tower:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ tower_motor:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ twil_origin:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Name: RobotModel
+ Robot Description: robot_description
+ TF Prefix: ""
+ Update Interval: 0
+ Value: true
+ Visual Enabled: 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: 42
+ Name: Odometry
+ Position Tolerance: 0.30000001192092896
+ Queue Size: 10
+ Shape:
+ Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Color: 255; 25; 0
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Value: Arrow
+ Topic: /twist_mrac_linearizing_controller/odom
+ Unreliable: false
+ Value: true
+ - Alpha: 0.699999988079071
+ Class: rviz/Map
+ Color Scheme: costmap
+ Draw Behind: false
+ Enabled: true
+ Name: Map
+ Topic: /move_base/global_costmap/costmap
+ Unreliable: false
+ Use Timestamp: 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_gmapping/Slam_map
+ Unreliable: false
+ Use Timestamp: false
+ Value: true
+ - Alpha: 1
+ Axes Length: 1
+ Axes Radius: 0.10000000149011612
+ Class: rviz/Pose
+ Color: 255; 255; 0
+ Enabled: true
+ Head Length: 0.30000001192092896
+ Head Radius: 0.10000000149011612
+ Name: Pose (goal)
+ Queue Size: 10
+ Shaft Length: 1
+ Shaft Radius: 0.05000000074505806
+ Shape: Arrow
+ Topic: /move_base/current_goal
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Buffer Length: 1
+ Class: rviz/Path
+ Color: 25; 255; 0
+ Enabled: true
+ Head Diameter: 0.30000001192092896
+ Head Length: 0.20000000298023224
+ Length: 0.30000001192092896
+ Line Style: Lines
+ Line Width: 0.029999999329447746
+ Name: Path
+ Offset:
+ X: 0
+ Y: 0
+ Z: 0
+ Pose Color: 255; 85; 255
+ Pose Style: None
+ Queue Size: 10
+ Radius: 0.029999999329447746
+ Shaft Diameter: 0.10000000149011612
+ Shaft Length: 0.10000000149011612
+ Topic: /move_base/TrajectoryPlannerROS/global_plan
+ Unreliable: false
+ Value: true
+ - Alpha: 1
+ Auto Size:
+ Auto Size Factor: 1
+ Value: true
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/DepthCloud
+ Color: 255; 255; 255
+ Color Image Topic: /camera/color/image_raw
+ Color Transformer: RGB8
+ Color Transport Hint: raw
+ Decay Time: 0
+ Depth Map Topic: /camera/depth/image_raw
+ Depth Map Transport Hint: raw
+ 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
+ Value: false
+ Position Transformer: XYZ
+ Queue Size: 5
+ Selectable: true
+ Size (Pixels): 3
+ Style: Flat Squares
+ Topic Filter: true
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: true
+ - Class: rviz/Camera
+ Enabled: true
+ Image Rendering: background and overlay
+ Image Topic: /camera/depth/image_raw
+ Name: Camera (depth)
+ Overlay Alpha: 0.5
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ Visibility:
+ Camera (color): true
+ DepthCloud: true
+ Grid: true
+ Map: true
+ Odometry: true
+ Path: true
+ Pose (goal): true
+ RobotModel: true
+ Value: true
+ Zoom Factor: 1
+ - Class: rviz/Camera
+ Enabled: true
+ Image Rendering: background and overlay
+ Image Topic: /camera/color/image_raw
+ Name: Camera (color)
+ Overlay Alpha: 0.5
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ Visibility:
+ Camera (depth): true
+ DepthCloud: true
+ Grid: true
+ Map: true
+ Odometry: true
+ Path: true
+ Pose (goal): true
+ RobotModel: true
+ Value: true
+ Zoom Factor: 1
+ Enabled: true
+ Global Options:
+ Background Color: 48; 48; 48
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 8.200119972229004
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: 0.19336318969726562
+ Y: 2.774914026260376
+ Z: 2.59694242477417
+ Focal Shape Fixed Size: true
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 1.4347964525222778
+ Target Frame: <Fixed Frame>
+ Yaw: 4.71176290512085
+ Saved: ~
+Window Geometry:
+ Camera (color):
+ collapsed: false
+ Camera (depth):
+ collapsed: false
+ Displays:
+ collapsed: false
+ Height: 1025
+ Hide Left Dock: false
+ Hide Right Dock: true
+ QMainWindow State: 000000ff00000000fd00000004000000000000015600000367fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001fb000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c00430061006d00650072006100200028006400650070007400680029010000023c000000980000001600fffffffb0000001c00430061006d006500720061002000280063006f006c006f0072002901000002da000000c80000001600ffffff000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002c400fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ Selection:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1853
+ X: 59
+ Y: 4
--- /dev/null
+#include <ros/ros.h>
+
+#include <Eigen/Dense>
+
+#include <std_msgs/Float64.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <sensor_msgs/LaserScan.h>
+#include <nav_msgs/Odometry.h>
+
+#include <kdl_parser/kdl_parser.hpp>
+
+class location
+{
+ public:
+ location(ros::NodeHandle node);
+ ~location(void);
+ void dataFusion(void);
+
+ private:
+ ros::NodeHandle node_;
+
+ ros::Subscriber visualodometrySubscriber;
+ ros::Subscriber odometrySubscriber;
+ ros::Publisher fusedOdomPublisher;
+
+ const int nJoints;
+
+ Eigen::VectorXd u;
+ Eigen::VectorXd thetaEst1;
+ Eigen::VectorXd thetaEst2;
+ Eigen::MatrixXd P1;
+ Eigen::MatrixXd P2;
+
+ ros::Time lastTime;
+
+ std::vector<double> wheelRadius;
+ double wheelBase;
+
+ nav_msgs::Odometry _odometry;
+ nav_msgs::Odometry _rgbd_odometry;
+
+ void VisualOdomCB(const nav_msgs::Odometry::ConstPtr &rgbd_odometry);
+ void OdomCB(const nav_msgs::Odometry::ConstPtr &odometry);
+};
+
+
+location::location(ros::NodeHandle node):
+ nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),wheelRadius(nJoints),_odometry()
+{
+ 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);
+
+ lastTime=ros::Time::now();
+
+}
+
+location::~location(void)
+{
+ visualodometrySubscriber.shutdown();
+ odometrySubscriber.shutdown();
+}
+
+void location::OdomCB(const nav_msgs::Odometry::ConstPtr &odometry)
+{
+ _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;
+}
+
+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.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);
+}
+
+
+int main(int argc,char* argv[])
+{
+ 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();
+ }
+ return 0;
+}
projects / twil.git / commitdiff