Merge branch 'add_tf_listener_broadcaster' into 'kinetic_migration'

Add tf listener broadcaster

See merge request labrobotica/ros/iri_core/scripts-catkin!3

add_tf_listener_broadcaster.sh

+#!/bin/bash
+
+# # WET
+
+script_name="add_tf_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 "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_tf_broadcaster.sh"
+source "${IRI_ROS_SCRIPTS_PATH}/libraries/create_tf_listener.sh"
+
+check_libraries
+check_templates
+
+echo ""
+echo "/*********************************************/"
+echo "/*   Creating New ROS TF Listener broadcaster */"
+echo "/*********************************************/"
+
+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 "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 "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 "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 "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 "Problems with headers and/or source files"
+fi
+
+#go to package folder
+roscd "${ros_pkg}"
+
+#modify node files adding server/client parameters
+if [[ "${pub_subs}" = "broadcaster" ]]
+then
+  create_tf_broadcaster  ${ros_pkg} ${node_h} ${node_c} ${driver_alg}
+else
+  create_tf_listener     ${ros_pkg} ${node_h} ${node_c} ${driver_alg}
+fi
+

libraries/create_tf_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 "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 "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 "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 "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 "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 "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="tf::TransformBroadcaster ${broadcaster_name};"
+  find_comment_in_file "${line}" "${node_h}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "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 "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 "A message with the same name is already published in file ${node_c} line ${line_number}"
+  fi
+}
+
+function create_tf_broadcaster
+{
+  #read input params
+  local ros_pkg=$1
+  local node_h=$2
+  local node_c=$3
+  local driver_alg=$4
+  local broadcaster_name="tf_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 "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 <tf/transform_broadcaster.h>"
+  comment="\[publisher subscriber headers\]"
+  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="tf::TransformBroadcaster tf_broadcaster_;"
+  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="//tf_broadcaster example"
+  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="t.rotation = tf::createQuaternionMsgFromYaw(0.0);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->transform_msg_.transform = t;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="this->tf_broadcaster_.sendTransform(this->transform_msg_);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="///tf_broadcaster example"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="*/"
+  line="${line}\ \ ${aux_line}\n"
+  
+  add_line_to_file "  ${line}" "${comment}" "${node_c}"
+
+################################################################################
+# Modify package.xml
+# check if the message package is the current ros package
+
+  add_build_run_dependencies "${driver_alg}" "${ros_pkg}" "tf"
+
+################################################################################
+# modify the CMakeLists.txt file
+
+  add_cmake_dependencies "${driver_alg}" "${ros_pkg}" "tf"
+
+################################################################################
+#compile
+  goto_catkin_workspace
+  catkin_make --only-pkg-with-deps ${ros_pkg}
+}

libraries/create_tf_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 "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 "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 "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 "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 "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 "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="tf::TransformListener ${listener_name};"
+  find_comment_in_file "${line}" "${node_h}"
+  if [[ "${comment_found}" = "true" ]]
+  then
+    kill_exit "A TF listener with the same name is already declared in file ${node_h} line ${line_number}"
+  fi
+
+  # check node.cpp file
+  
+}
+
+function create_tf_listener
+{
+  #read input params
+  local ros_pkg=$1
+  local node_h=$2
+  local node_c=$3
+  local driver_alg=$4
+  local listener_name="tf_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 "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}"
+
+################################################################################
+# modify Node.h #
+
+  line="#include <tf/transform_listener.h>"
+  comment="\[publisher subscriber headers\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+
+  aux_line="tf::TransformListener ${listener_name};"
+  line="\ \ \ \ ${aux_line}\n"
+  comment="\[subscriber attributes\]"
+  add_line_to_file "${line}" "${comment}" "${node_h}"
+
+################################################################################
+# modify Node.cpp #
+
+  comment="\[fill msg structures\]"
+  
+  line="\n"
+  aux_line="/*"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="//tf_listener example"
+  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="  ros::Duration polling_sleep_duration = ros::Duration(0.01);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  bool tf_exists = this->tf_listener_.waitForTransform(target_frame, source_frame, time, timeout, polling_sleep_duration);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  if(tf_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="    stamped_pose_in.pose.orientation = tf::createQuaternionMsgFromYaw(0.0);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"Original    pose in '%s' frame, with (x,y,z)=[%f,%f,%f], yaw=[%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, tf::getYaw(stamped_pose_in.pose.orientation));"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    geometry_msgs::PoseStamped stamped_pose_out;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    this->tf_listener_.transformPose(target_frame, stamped_pose_in, stamped_pose_out);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"Transformed pose in '%s'  frame, with (x,y,z)=[%f,%f,%f], yaw=[%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, tf::getYaw(stamped_pose_out.pose.orientation));"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"---\");"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="\n"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf::StampedTransform tf_parent_child;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf::Transform tf_parent_point, tf_child_point;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    this->tf_listener_.lookupTransform(source_frame, target_frame, time, tf_parent_child);"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf_parent_point.setOrigin(tf::Vector3(stamped_pose_in.pose.position.x, stamped_pose_in.pose.position.y, stamped_pose_in.pose.position.z));"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf_parent_point.setRotation(tf::Quaternion(stamped_pose_in.pose.orientation.x, stamped_pose_in.pose.orientation.y, stamped_pose_in.pose.orientation.z, stamped_pose_in.pose.orientation.w));"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    tf_child_point = tf_parent_child.inverse()*tf_parent_point;"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="    ROS_INFO(\"Transformed pose in '%s'  frame, with (x,y,z)=[%f,%f,%f], yaw=[%f]\", target_frame.c_str(), tf_child_point.getOrigin().x(), tf_child_point.getOrigin().y(), tf_child_point.getOrigin().z(), tf::getYaw(tf_child_point.getRotation()));"
+  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 (tf::TransformException &ex){"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="  ROS_ERROR(\"TF Exception: %s\",ex.what()); }"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="///tf_listener example"
+  line="${line}\ \ ${aux_line}\n"
+  aux_line="*/"
+  line="${line}\ \ ${aux_line}\n"
+  
+  add_line_to_file "  ${line}" "${comment}" "${node_c}"
+
+################################################################################
+# Modify package.xml
+# check if the message package is the current ros package
+
+  add_build_run_dependencies "${driver_alg}" "${ros_pkg}" "tf"
+
+################################################################################
+# modify the CMakeLists.txt file
+# check if the message package is the current ros package
+
+  add_cmake_dependencies "${driver_alg}" "${ros_pkg}" "tf"
+
+################################################################################
+#compile
+  goto_catkin_workspace
+  catkin_make --only-pkg-with-deps ${ros_pkg}
+}

libraries/scripts_library.sh

@@ -21,7 +21,7 @@ function check_libraries
   if [[ -d "${IRI_ROS_SCRIPTS_PATH}/libraries" ]]
   then
     pushd "${IRI_ROS_SCRIPTS_PATH}/libraries"
-    if [[ -e "create_server.sh" ]] && [[ -e "create_client.sh" ]] && [[ -e "create_publisher.sh" ]] && [[ -e "create_subscriber.sh" ]]
+    if [[ -e "create_server.sh" ]] && [[ -e "create_client.sh" ]] && [[ -e "create_publisher.sh" ]] && [[ -e "create_subscriber.sh" ]] && [[ -e "create_tf_broadcaster.sh" ]] && [[ -e "create_tf_listener.sh" ]]
     then
       echo "All library files available"
       popd