diff --git a/release_packages.yaml b/release_packages.yaml index 6a1843e..b7043f7 100644 --- a/release_packages.yaml +++ b/release_packages.yaml @@ -1 +1,2 @@ - tf_lat_lon +- tf_convenience_topics diff --git a/smarc_navigation.rosinstall b/smarc_navigation.rosinstall index 4d90630..4dd2fff 100644 --- a/smarc_navigation.rosinstall +++ b/smarc_navigation.rosinstall @@ -2,3 +2,4 @@ - git: {local-name: smarc_msgs, uri: 'https://github.com/smarc-project/smarc_msgs.git', version: noetic-devel} - git: {local-name: sbg_ros_driver, uri: 'https://github.com/smarc-project/sbg_ros_driver.git', version: master} - git: {local-name: geographic_info, uri: 'https://github.com/ros-geographic-info/geographic_info.git', version: master} + diff --git a/tf_convenience_topics/CMakeLists.txt b/tf_convenience_topics/CMakeLists.txt new file mode 100644 index 0000000..ace5166 --- /dev/null +++ b/tf_convenience_topics/CMakeLists.txt @@ -0,0 +1,230 @@ +cmake_minimum_required(VERSION 3.0.2) +project(tf_convenience_topics) + +## 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 + nav_msgs + roscpp + rospy + std_msgs + smarc_msgs + tf +) + +## 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 +# nav_msgs# std_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 state_feedback +# CATKIN_DEPENDS nav_msgs roscpp rospy std_msgs tf +# 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} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/state_feedback.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}_node src/state_feedback_node.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}_node +# ${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_state_feedback.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) + +add_executable(odom_listener src/odom_listener.cpp) +add_executable(tf_listener src/tf_listener.cpp) + +add_dependencies(odom_listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(tf_listener ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +target_link_libraries(odom_listener ${catkin_LIBRARIES}) +target_link_libraries(tf_listener ${catkin_LIBRARIES}) + +install(TARGETS tf_listener odom_listener + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(FILES + launch/tf_listener.launch + launch/odom_listener.launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + diff --git a/tf_convenience_topics/launch/odom_listener.launch b/tf_convenience_topics/launch/odom_listener.launch new file mode 100644 index 0000000..34b4b28 --- /dev/null +++ b/tf_convenience_topics/launch/odom_listener.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/tf_convenience_topics/launch/tf_listener.launch b/tf_convenience_topics/launch/tf_listener.launch new file mode 100644 index 0000000..b8e40a2 --- /dev/null +++ b/tf_convenience_topics/launch/tf_listener.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/tf_convenience_topics/package.xml b/tf_convenience_topics/package.xml new file mode 100644 index 0000000..3a9ce5c --- /dev/null +++ b/tf_convenience_topics/package.xml @@ -0,0 +1,74 @@ + + + tf_convenience_topics + 0.0.0 + Package to provide state feedback by looking at the tf tree + + + + + svbhat + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + nav_msgs + roscpp + rospy + std_msgs + tf + nav_msgs + roscpp + rospy + std_msgs + tf + nav_msgs + roscpp + rospy + std_msgs + tf + + + + + + + + diff --git a/tf_convenience_topics/src/odom_listener.cpp b/tf_convenience_topics/src/odom_listener.cpp new file mode 100644 index 0000000..e0c59b6 --- /dev/null +++ b/tf_convenience_topics/src/odom_listener.cpp @@ -0,0 +1,109 @@ +//Listener Node to the Odometry topic to read positions and velocities in the local frame for SAM, Sriharsha Bhat, 29.5.2020 +#include +#include +#include +#include +#include +#include + +//Variable initialization +std_msgs::Float64 current_roll,current_pitch,current_yaw,current_depth, current_x,current_y, current_u, current_v, current_w, current_p,current_q, current_r, current_alt; +double r,p,y; +tf::Quaternion tfq; + +//Callback to read altitude from DVL +void DVLCallback(const smarc_msgs::DVL& dvl_msg) +{ + current_alt.data = dvl_msg.altitude; + // ROS_INFO_THROTTLE(1.0, "[ odom_listener] Altitude from DVL: %f", dvl_msg.altitude); +} + +//Callback to read states from odomety +void OdomCallback(const nav_msgs::Odometry& odom_msg) +{ + tf::quaternionMsgToTF(odom_msg.pose.pose.orientation,tfq); + + tf::Matrix3x3(tfq).getEulerYPR(y,p,r); + + // TODO: Check signs? + //orientation + current_pitch.data= p; + current_roll.data= r; + current_yaw.data= y; + current_depth.data= odom_msg.pose.pose.position.z; + current_x.data= odom_msg.pose.pose.position.x; + current_y.data= odom_msg.pose.pose.position.y; + + //Velocity + current_u.data= odom_msg.twist.twist.linear.x; + current_v.data= odom_msg.twist.twist.linear.y; + current_w.data= odom_msg.twist.twist.linear.z; + current_p.data= odom_msg.twist.twist.angular.x; + current_q.data= odom_msg.twist.twist.angular.y; + current_r.data= odom_msg.twist.twist.angular.z; +} + +int main(int argc, char** argv){ + + std::string node_name = "odom_listener"; + ros::init(argc, argv, node_name); + + ros::NodeHandle node("~"); + + std::string odom_topic_; + std::string dvl_topic_; + double freq; + + node.param("odom_topic", odom_topic_, "/sam/dr/local/odom/filtered"); + node.param("dvl_topic", dvl_topic_, "/sam/core/dvl"); + node.param("loop_freq", freq, 10); + + //initiate subscribers + ros::Subscriber dvl_sub = node.subscribe(dvl_topic_, 1, DVLCallback); + ros::Subscriber odom_sub = node.subscribe(odom_topic_, 1, OdomCallback); + + //initiate publishers + ros::Publisher feedback_pitch = node.advertise("pitch_feedback", freq); + ros::Publisher feedback_roll = node.advertise("roll_feedback", freq); + ros::Publisher feedback_yaw = node.advertise("yaw_feedback", freq); + ros::Publisher feedback_depth = node.advertise("depth_feedback", freq); + ros::Publisher feedback_x = node.advertise("x_feedback", freq); + ros::Publisher feedback_y = node.advertise("y_feedback", freq); + ros::Publisher feedback_u = node.advertise("u_feedback", freq); + ros::Publisher feedback_v = node.advertise("v_feedback", freq); + ros::Publisher feedback_w = node.advertise("w_feedback", freq); + ros::Publisher feedback_p = node.advertise("p_feedback", freq); + ros::Publisher feedback_q = node.advertise("q_feedback", freq); + ros::Publisher feedback_r = node.advertise("r_feedback", freq); + ros::Publisher feedback_alt = node.advertise("alt_feedback", freq); + + ros::Rate rate(10.0); + while (node.ok()){ + + //Publish orientation + feedback_pitch.publish(current_pitch); + feedback_roll.publish(current_roll); + feedback_yaw.publish(current_yaw); + feedback_depth.publish(current_depth); + feedback_x.publish(current_x); + feedback_y.publish(current_y); + + //Publish velocity + feedback_u.publish(current_u); + feedback_v.publish(current_v); + feedback_w.publish(current_w); + feedback_p.publish(current_p); + feedback_q.publish(current_q); + feedback_r.publish(current_r); + + //Publish DVL altitude + feedback_alt.publish(current_alt); + + + ROS_INFO_THROTTLE(1.0, "[ odom_listener ] roll: %f, pitch: %f, yaw: %f, depth: %f, x: %f, y: %f, u: %f, v: %f, w: %f, p: %f, q: %f, r: %f alt:%f", current_roll.data,current_pitch.data,current_yaw.data,current_depth.data, current_x.data, current_y.data, current_u.data, current_v.data, current_w.data, current_p.data, current_q.data, current_r.data, current_alt.data); + + rate.sleep(); + ros::spinOnce(); + } + return 0; +}; diff --git a/tf_convenience_topics/src/tf_listener.cpp b/tf_convenience_topics/src/tf_listener.cpp new file mode 100644 index 0000000..8867c35 --- /dev/null +++ b/tf_convenience_topics/src/tf_listener.cpp @@ -0,0 +1,130 @@ +//PID Transform Listener Node for SAM, Sriharsha Bhat, 7 Dec 2018 +#include +#include +//#include +#include +//#include +//#include +#include +#include + +std_msgs::Float64 current_alt; + +//Callback to read altitude from DVL +void DVLCallback(const smarc_msgs::DVL& dvl_msg) +{ + current_alt.data = dvl_msg.altitude; + ROS_INFO_THROTTLE(1.0, "[ tf_listener] Altitude from DVL: %f", dvl_msg.altitude); +} + +int main(int argc, char** argv){ + + std::string node_name = "tf_listener"; + ros::init(argc, argv, node_name); + + ros::NodeHandle node; + + std::string base_frame_; + std::string odom_frame_; + std::string map_frame_; + std::string topic_from_dvl_; + double freq; + + ros::NodeHandle node_priv("~"); + node_priv.param("base_frame", base_frame_, "/sam/base_link"); + node_priv.param("world_frame", map_frame_, "/world_ned"); + //node.param("odom_frame", odom_frame_, "/odom"); + node_priv.param("topic_from_dvl", topic_from_dvl_, "/sam/core/dvl"); + node_priv.param("loop_freq", freq, 10); + + //initiate subscribers + ros::Subscriber dvl_sub = node.subscribe(topic_from_dvl_, 1, DVLCallback); + +//initiate publishers + ros::Publisher feedback_pitch = node.advertise("pitch_feedback", freq); + ros::Publisher feedback_roll = node.advertise("roll_feedback", freq); + ros::Publisher feedback_yaw = node.advertise("yaw_feedback", freq); + ros::Publisher feedback_depth = node.advertise("depth_feedback", freq); + ros::Publisher feedback_x = node.advertise("x_feedback", freq); + ros::Publisher feedback_y = node.advertise("y_feedback", freq); + ros::Publisher feedback_u = node.advertise("u_feedback", freq); + ros::Publisher feedback_v = node.advertise("v_feedback", freq); + ros::Publisher feedback_w = node.advertise("w_feedback", freq); + ros::Publisher feedback_p = node.advertise("p_feedback", freq); + ros::Publisher feedback_q = node.advertise("q_feedback", freq); + ros::Publisher feedback_r = node.advertise("r_feedback", freq); + ros::Publisher feedback_alt = node.advertise("alt_feedback", freq); + +//Variable initialization + tf::TransformListener listener; + std_msgs::Float64 current_roll,current_pitch,current_yaw,current_depth, current_x,current_y, current_u, current_v, current_w, current_p,current_q, current_r; + double r,p,y; + tf::Quaternion tfq; + + //Define the transforms to make this modular : can be done once you have a working system + //node_priv.param("topic_from_controller", topic_from_controller_, "control_effort"); + + + ros::Rate rate(10.0); + while (node.ok()){ + tf::StampedTransform transform; + geometry_msgs::Twist twist; + try{//transform between utm and required frame + listener.lookupTransform(map_frame_, base_frame_, + ros::Time(0), transform); + listener.lookupTwist(map_frame_, base_frame_, + ros::Time(0), ros::Duration(2.0), twist); + } + + catch (tf::TransformException ex){ + ROS_ERROR("%s",ex.what()); + ros::Duration(1.0).sleep(); + } + + //get orientation with quaternions + tfq = transform.getRotation(); + tf::Matrix3x3(tfq).getEulerYPR(y,p,r); + + //orientation + current_pitch.data= p; + current_roll.data= r; + current_yaw.data= y; + current_depth.data= -transform.getOrigin().z(); + current_x.data= transform.getOrigin().x(); + current_y.data= transform.getOrigin().y(); + + //Velocity + current_u.data= twist.linear.x; + current_v.data= twist.linear.y; + current_w.data= twist.linear.z; + current_p.data= twist.angular.x; + current_q.data= twist.angular.y; + current_r.data= twist.angular.z; + + //Publish orientation + feedback_pitch.publish(current_pitch); + feedback_roll.publish(current_roll); + feedback_yaw.publish(current_yaw); + feedback_depth.publish(current_depth); + feedback_x.publish(current_x); + feedback_y.publish(current_y); + + //Publish velocity + feedback_u.publish(current_u); + feedback_v.publish(current_v); + feedback_w.publish(current_w); + feedback_p.publish(current_p); + feedback_q.publish(current_q); + feedback_r.publish(current_r); + + //Publish DVL altitude + feedback_alt.publish(current_alt); + + + ROS_INFO_THROTTLE(1.0, "[ tf_listener ] roll: %f, pitch: %f, yaw: %f, depth: %f, x: %f, y: %f, u: %f, v: %f, w: %f, p: %f, q: %f, r: %f alt:%f", current_roll.data,current_pitch.data,current_yaw.data,current_depth.data, current_x.data, current_y.data, current_u.data, current_v.data, current_w.data, current_p.data, current_q.data, current_r.data, current_alt.data); + + rate.sleep(); + ros::spinOnce(); + } + return 0; +};