Add script to add tf2 broadcaster/listener

add_tf2_listener_broadcaster.sh

+#!/bin/bash
+
+echo "/*********************************************/"
+echo "/*   Creating New ROS TF2 Listener broadcaster */"
+echo "/*********************************************/"
+
+script_name="add_tf2_listener_broadcaster"
+
+# check wether the scripts path environment variable has been defined
+scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
+if [[ -z "${scripts_path}" ]]
+then
+  echo "ERROR: The scripts path environment varibale has not been defined. Please see the wiki documentation for instructions on how to create it."
+  exit
+# else
+#   echo "The scripts path environment variable has been properly defined."
+fi
+
+source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
+source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_tf2_broadcaster.sh"
+source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_tf2_listener.sh"
+
+check_libraries
+check_templates
+
+pub_subs=
+ros_pkg=
+
+#check for input project name paramenter
+while getopts “:o:p:” OPTION
+do
+  case $OPTION in
+    o)
+       pub_subs=$OPTARG
+       ;;
+    p)
+       ros_pkg=$OPTARG
+       ;;
+
+    ?)
+       echo "ERROR: invalid input argument ${OPTION}"
+       kill_exit "Usage: ${script_name}.sh -o [broadcaster,listener] -p ros_pkg"
+       exit
+       ;;
+  esac
+done
+
+#check if publisher name parameter is filled up
+if [ ! "${pub_subs}" ] || [ ! "${ros_pkg}" ]
+then
+  echo "ERROR: Missing input parameters..."
+  kill_exit "Usage: ${script_name}.sh -o [broadcaster,listener] -p ros_pkg"
+fi
+
+#check publisher subscriber parameter
+if [[ ! "${pub_subs}" = "broadcaster" ]] && [[ ! "${pub_subs}" = "listener" ]]
+then
+  kill_exit "ERROR: First parameter must be either \"broadcaster\" or \"listener\", aborting ..."
+fi
+
+#check if package exists
+result=`roscd ${ros_pkg}`
+if [[ -z "${result}" ]]
+then
+  roscd ${ros_pkg}
+else
+  kill_exit "ERROR: ROS package ${ros_pkg} does NOT exist or can't be found. Create it, or load the correct workspace."
+fi
+
+check_package "${ros_pkg}"
+if [[ ${pkg_exists} == true ]]
+then
+  roscd ${ros_pkg}
+else
+  kill_exit "ROS package ${ros_pkg} does NOT exist of can't be found. Create it, or load the correct workspace."
+fi
+
+#retrieve header and source files and pkg kind (algorithm/driver)
+get_h_cpp_files ${ros_pkg}
+is_driver_or_alg_node ${ros_pkg}
+echo "node_h=${node_h}"
+echo "node_c=${node_c}"
+echo "driver_alg=${driver_alg}"
+
+if [[ -z ${node_h} ]] || [[ -z ${node_c} ]] || [[ -z ${driver_alg} ]]
+then
+  kill_exit "ERROR: Problems found with headers and/or source files"
+fi
+
+#go to package folder
+roscd "${ros_pkg}"
+
+#modify readme with added ROS interface
+type="topic"
+fill_readme_ros_interface ${type} ${pub_subs} ${ros_pkg} "tf" "tf" "tfMessage" "true"
+
+#modify node files adding server/client parameters
+if [[ "${pub_subs}" = "broadcaster" ]]
+then
+  create_tf2_broadcaster  ${ros_pkg} ${node_h} ${node_c} ${driver_alg}
+else
+  create_tf2_listener     ${ros_pkg} ${node_h} ${node_c} ${driver_alg}
+fi

libraries/create_tf2_broadcaster.sh

+#!/bin/bash
+
+# check wether the scripts path environment variable has been defined
+scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
+if [[ -z "${scripts_path}" ]]
+then
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  exit
+# else
+#   echo "The scripts path environment variable has been properly defined."
+fi
+
+source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
+
+function check_publisher_file_integrity
+{
+  local node_h=$1
+  local node_c=$2
+  local comment=""
+
+  # check the node.h file
+  comment="\[publisher subscriber headers\]"
+  find_comment_in_file "${comment}" "${node_h}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[publisher subscriber headers\] from the header file ${node_h}"
+  fi
+  comment="\[publisher attributes\]"
+  find_comment_in_file "${comment}" "${node_h}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[publisher attributes\] from the header file ${node_h}"
+  fi
+
+  # check the node.cpp file
+  comment="\[init publishers\]"
+  find_comment_in_file "${comment}" "${node_c}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[init publishers\] from the header file ${node_c}"
+  fi
+  comment="\[fill msg structures\]"
+  find_comment_in_file "${comment}" "${node_c}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[fill msg structures\] from the header file ${node_c}"
+  fi
+
+  comment="\[publish messages\]"
+  find_comment_in_file "${comment}" "${node_c}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[publish messages\] from the header file ${node_c}"
+  fi
+}
+
+function check_publisher_attributes_functions
+{
+  local node_h=$1
+  local node_c=$2
+  local broadcaster_name=$3
+  local line=""
+
+  # check the node.h file
+  line="tf2_ros::TransformBroadcaster ${broadcaster_name};"
+  find_comment_in_file "${line}" "${node_h}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "ERROR: A TF broadcaster with the same name is already declared in file ${node_h} line ${line_number}"
+  fi
+
+  line="geometry_msgs::TransformStampd transform_msg;"
+  find_comment_in_file "${line}" "${node_h}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "ERROR: A message variable with the same name is already declared in file ${node_h} line ${line_number}"
+  fi
+
+  # check the node.cpp file
+  line="this->${broadcaster_name}.sendTransform(this->transform_msg);"
+  find_comment_in_file "${line}" "${node_c}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "ERROR: A message with the same name is already published in file ${node_c} line ${line_number}"
+  fi
+}
+
+function create_tf2_broadcaster
+{
+  #read input params
+  local ros_pkg=$1
+  local node_h=$2
+  local node_c=$3
+  local driver_alg=$4
+  local broadcaster_name="tf2_broadcaster"
+  local line=""
+  local old_line=""
+  local aux_line=""
+  local comment=""
+  local old_string=""
+  local new_string=""
+
+  #get class basename
+  get_class_basename "${node_c}"
+  if [[ -z ${class_name} ]]
+  then
+    kill_exit "ERROR: impossible to retrieve class basename"
+  fi
+
+  #check files integrity before making any changes
+  check_package_file_integrity "${driver_alg}"
+  check_cmakelists_file_integrity "${driver_alg}"
+  check_publisher_file_integrity "${node_h}" "${node_c}"
+  check_publisher_attributes_functions "${node_h}" "${node_c}" "${broadcaster_name}"
+
+################################################################################
+# modify Node.h #
+
+  #look for include files and add them if are not already there
+  line="#include <tf2_ros/transform_broadcaster.h>"
+  comment="\[publisher subscriber headers\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+  line="#include <tf2_geometry_msgs/tf2_geometry_msgs.h>"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+  # add the message type variable
+  aux_line="geometry_msgs::TransformStamped transform_msg;"
+  line="\ \ \ \ ${aux_line}\n"
+  comment="\[publisher attributes\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+  aux_line="tf2_ros::TransformBroadcaster ${broadcaster_name};"
+  line="\ \ \ \ ${aux_line}"
+  comment="\[publisher attributes\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+
+################################################################################
+# modify Node.cpp #
+
+  comment="\[publish messages\]"
+
+  line="\n"
+  aux_line="/*"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="//tf2_broadcaster example BEGIN"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->transform_msg.header.stamp    = ros::Time::now();"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->transform_msg.header.frame_id = \"parent_frame\";"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->transform_msg.child_frame_id  = \"child_frame\";"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="geometry_msgs::Transform t;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="t.translation.x = 0.0;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="t.translation.y = 0.0;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="t.translation.z = 0.0;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="tf2::Quaternion q;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="q.setRPY(0.0, 0.0, 0.0);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="t.rotation = tf2::toMsg(q);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->transform_msg.transform = t;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->${broadcaster_name}.sendTransform(this->transform_msg);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="//tf2_broadcaster example END"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="*/"
+  line="${line}\ \ ${aux_line}\n"
+  
+  add_line_to_file "  ${line}" "${comment}" "${node_c}"
+
+################################################################################
+# Modify package.xml
+
+  add_build_run_dependencies "${driver_alg}" "${ros_pkg}" "tf2_ros"
+
+################################################################################
+# modify the CMakeLists.txt file
+
+  add_cmake_dependencies "${driver_alg}" "${ros_pkg}" "tf2_ros"
+
+################################################################################
+#compile
+  goto_catkin_workspace
+  catkin_make --only-pkg-with-deps ${ros_pkg}
+}

libraries/create_tf2_listener.sh

+#!/bin/bash
+
+# check wether the scripts path environment variable has been defined
+scripts_path=`echo "${IRI_ROS_SCRIPTS_PATH}"`
+if [[ -z "${scripts_path}" ]]
+then
+  echo "ERROR: The scripts path environment variable has not been defined. Please see the wiki documentation for instructions on how to create it."
+  exit
+# else
+#   echo "The scripts path environment variable has been properly defined."
+fi
+
+source "${IRI_ROS_SCRIPTS_PATH}/libraries/scripts_library.sh"
+
+function check_subscriber_file_integrity
+{
+  local node_h=$1
+  local node_c=$2
+  local comment=""
+
+  # check node.h file
+  comment="\[publisher subscriber headers\]"
+  find_comment_in_file "${comment}" "${node_h}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[publisher subscriber headers\] from the header file ${node_h}"
+  fi
+  comment="\[subscriber attributes\]"
+  find_comment_in_file "${comment}" "${node_h}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[subscriber attributes\] from the header file ${node_h}"
+  fi
+
+  # check node.cpp file
+  comment="\[init subscribers\]" 
+  find_comment_in_file "${comment}" "${node_c}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[init subscribers\] from the header file ${node_c}"
+  fi
+  comment="\[subscriber callbacks\]"
+  find_comment_in_file "${comment}" "${node_c}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[subscriber callbacks\] from the header file ${node_c}"
+  fi
+  comment="\[free dynamic memory\]"
+  find_comment_in_file "${comment}" "${node_c}"
+  if [[ "${comment_found}" = "false" ]]
+  then
+    kill_exit "ERROR: Missing \[free dynamic memory\] from the header file ${node_c}"
+  fi
+}
+
+function check_subscriber_attributes_functions
+{
+  local node_h=$1
+  local node_c=$2
+  local listener_name=$3
+  local class_name=$4
+  local line=""
+
+  # check node.h file
+
+  line="tf2_ros::TransformListener ${listener_name};"
+  find_comment_in_file "${line}" "${node_h}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "ERROR: A TF2 listener with the same name is already declared in file ${node_h} line ${line_number}"
+  fi
+  
+  line="tf2_ros::Buffer tf2_buffer;"
+  find_comment_in_file "${line}" "${node_h}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "ERROR: A TF2 buffer with the same name is already declared in file ${node_h} line ${line_number}"
+  fi
+
+  # check node.cpp file
+  
+}
+
+function create_tf2_listener
+{
+  #read input params
+  local ros_pkg=$1
+  local node_h=$2
+  local node_c=$3
+  local driver_alg=$4
+  local listener_name="tf2_listener"
+  local line=""
+  local old_line=""
+  local aux_line=""
+  local comment=""
+  local old_string=""
+  local new_string=""
+
+  #get class basename
+  get_class_basename "${node_c}"
+  if [[ -z ${class_name} ]]
+  then
+    kill_exit "ERROR: impossible to retrieve class basename"
+  fi
+# echo "class_name=${class_name}"
+
+  #check files integrity before making any changes
+  check_package_file_integrity "${driver_alg}"
+  check_cmakelists_file_integrity "${driver_alg}"
+  check_subscriber_file_integrity "${node_h}" "${node_c}"
+  check_subscriber_attributes_functions "${node_h}" "${node_c}" "${listener_name}" "${class_name}"
+
+  local obj=""
+  if [[ "${driver_alg}" = "driver" ]]
+  then
+    obj="this->driver_."
+  else
+    obj="this->alg_."
+  fi
+
+  ################################################################################
+  # modify Node.h #
+
+  line="#include <tf2_ros/transform_listener.h>"
+  comment="\[publisher subscriber headers\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+  line="#include <tf2_geometry_msgs/tf2_geometry_msgs.h>"
+  comment="\[publisher subscriber headers\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+
+  aux_line="tf2_ros::TransformListener ${listener_name};"
+  line="\ \ \ \ ${aux_line}\n"
+  comment="\[subscriber attributes\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+  
+  aux_line="tf2_ros::Buffer tf2_buffer;"
+  line="\ \ \ \ ${aux_line}\n"
+  comment="\[subscriber attributes\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+################################################################################
+# modify Node.cpp #
+
+  #Add constructor
+  if [[ "${driver_alg}" = "driver" ]]
+  then
+    comment="iri_base_driver::IriBaseNodeDriver<"
+  else
+    comment="algorithm_base::IriBaseAlgorithm<"
+  fi
+           
+  line=",${listener_name}(tf2_buffer)"
+  add_line_to_file "  ${line}" "${comment}" "${node_c}"
+  
+  #Add example
+  
+  comment="\[fill msg structures\]"
+  
+  line="\n"
+  aux_line="/*"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="//tf2_listener example BEGIN"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="try{"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  std::string target_frame             = \"child_frame\";"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  std::string source_frame             = \"parent_frame\";"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  ros::Time time                       = ros::Time::now();"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  ros::Duration timeout                = ros::Duration(0.1);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  ${obj}unlock();"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  bool tf2_exists = this->tf2_buffer.canTransform(target_frame, source_frame, time, timeout);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  ${obj}lock();"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  if(tf2_exists){"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    geometry_msgs::PoseStamped stamped_pose_in;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    stamped_pose_in.header.stamp     = time;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    stamped_pose_in.header.frame_id  = source_frame;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    stamped_pose_in.pose.position.x    = 1.0;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    stamped_pose_in.pose.position.y    = 0.0;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    stamped_pose_in.pose.position.z    = 0.0;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf2::Quaternion quat_tf;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    quat_tf.setRPY(0.0, 0.0, 1.5709);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    geometry_msgs::Quaternion quat_msg;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf2::convert(quat_tf, quat_msg);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    stamped_pose_in.pose.orientation = quat_msg;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    double yaw, pitch, roll;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"Original    pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]\", stamped_pose_in.header.frame_id.c_str(), stamped_pose_in.pose.position.x, stamped_pose_in.pose.position.y, stamped_pose_in.pose.position.z, yaw, pitch, roll);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    geometry_msgs::PoseStamped stamped_pose_out;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    geometry_msgs::TransformStamped transform;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    transform = this->tf2_buffer.lookupTransform(target_frame, source_frame, time);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    quat_msg = transform.transform.rotation;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"Found transform betwen frames (%s-->%s) with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]\", source_frame.c_str(), target_frame.c_str(), transform.transform.translation.x, transform.transform.translation.y, transform.transform.translation.z, yaw, pitch, roll);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf2::doTransform(stamped_pose_in, stamped_pose_out, transform);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    quat_msg = stamped_pose_out.pose.orientation;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf2::Matrix3x3(tf2::Quaternion(quat_msg.x, quat_msg.y,quat_msg.z, quat_msg.w)).getEulerYPR(yaw, pitch, roll);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"Transformed pose in '%s' frame, with (x,y,z)=[%f,%f,%f], (yaw,pitch,roll)=[%f,%f,%f]\", stamped_pose_out.header.frame_id.c_str(), stamped_pose_out.pose.position.x, stamped_pose_out.pose.position.y, stamped_pose_out.pose.position.z, yaw, pitch, roll);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"---\");"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  }else{"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_WARN(\"No transform found from '%s' to '%s'\", source_frame.c_str(), target_frame.c_str()); }"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="}catch (tf2::TransformException &ex){"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  ROS_ERROR(\"TF2 Exception: %s\",ex.what()); }"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="//tf2_listener example END"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="*/"
+  line="${line}\ \ ${aux_line}\n"
+  
+  add_line_to_file "  ${line}" "${comment}" "${node_c}"
+
+################################################################################
+# Modify package.xml
+
+  add_build_run_dependencies "${driver_alg}" "${ros_pkg}" "tf2_ros"
+
+################################################################################
+# modify the CMakeLists.txt file
+
+  add_cmake_dependencies "${driver_alg}" "${ros_pkg}" "tf2_ros"
+
+################################################################################
+#compile
+  goto_catkin_workspace
+  catkin_make --only-pkg-with-deps ${ros_pkg}
+}