From f1e1714fdbfb9972f04fb1f9f2b714bc70c05c19 Mon Sep 17 00:00:00 2001 From: Raul Cataluna Athayde Date: Fri, 19 Mar 2021 21:07:17 -0300 Subject: [PATCH] twil_location added --- twil_location/CMakeLists.txt | 206 +++++++++++++++ twil_location/launch/gazebo.launch | 41 +++ twil_location/launch/location.launch | 64 +++++ twil_location/launch/new_world.launch | 18 ++ twil_location/launch/plot.launch | 15 ++ twil_location/package.xml | 62 +++++ twil_location/rviz/twil_mapping.rviz | 467 ++++++++++++++++++++++++++++++++++ twil_location/src/twil_location.cpp | 145 +++++++++++ 8 files changed, 1018 insertions(+) create mode 100644 twil_location/CMakeLists.txt create mode 100644 twil_location/launch/gazebo.launch create mode 100644 twil_location/launch/location.launch create mode 100644 twil_location/launch/new_world.launch create mode 100644 twil_location/launch/plot.launch create mode 100644 twil_location/package.xml create mode 100644 twil_location/rviz/twil_mapping.rviz create mode 100644 twil_location/src/twil_location.cpp diff --git a/twil_location/CMakeLists.txt b/twil_location/CMakeLists.txt new file mode 100644 index 0000000..fbb762b --- /dev/null +++ b/twil_location/CMakeLists.txt @@ -0,0 +1,206 @@ +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) diff --git a/twil_location/launch/gazebo.launch b/twil_location/launch/gazebo.launch new file mode 100644 index 0000000..78e45df --- /dev/null +++ b/twil_location/launch/gazebo.launch @@ -0,0 +1,41 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_location/launch/location.launch b/twil_location/launch/location.launch new file mode 100644 index 0000000..6924902 --- /dev/null +++ b/twil_location/launch/location.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_location/launch/new_world.launch b/twil_location/launch/new_world.launch new file mode 100644 index 0000000..b6a4011 --- /dev/null +++ b/twil_location/launch/new_world.launch @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/twil_location/launch/plot.launch b/twil_location/launch/plot.launch new file mode 100644 index 0000000..eb277a4 --- /dev/null +++ b/twil_location/launch/plot.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/twil_location/package.xml b/twil_location/package.xml new file mode 100644 index 0000000..e9058a6 --- /dev/null +++ b/twil_location/package.xml @@ -0,0 +1,62 @@ + + + twil_location + 0.0.0 + The twil_location package + + + + + raul + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + roscpp + roscpp + + + + + + + + diff --git a/twil_location/rviz/twil_mapping.rviz b/twil_location/rviz/twil_mapping.rviz new file mode 100644 index 0000000..114a21a --- /dev/null +++ b/twil_location/rviz/twil_mapping.rviz @@ -0,0 +1,467 @@ +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: + 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: + 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 diff --git a/twil_location/src/twil_location.cpp b/twil_location/src/twil_location.cpp new file mode 100644 index 0000000..2b809bb --- /dev/null +++ b/twil_location/src/twil_location.cpp @@ -0,0 +1,145 @@ +#include + +#include + +#include +#include +#include +#include + +#include + +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 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("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; +} -- 2.12.0