diff --git a/cob_control/package.xml b/cob_control/package.xml index 65a33dd0f..5f57e2027 100644 --- a/cob_control/package.xml +++ b/cob_control/package.xml @@ -15,7 +15,6 @@ cob_base_velocity_smoother cob_collision_velocity_filter cob_footprint_observer - cob_hardware_emulation diff --git a/cob_hardware_emulation/CHANGELOG.rst b/cob_hardware_emulation/CHANGELOG.rst deleted file mode 100644 index 8b810d8f8..000000000 --- a/cob_hardware_emulation/CHANGELOG.rst +++ /dev/null @@ -1,267 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package cob_hardware_emulation -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.9.25 (2024-08-05) -------------------- - -0.9.24 (2024-04-30) -------------------- -* 0.8.24 -* update changelogs -* Contributors: fmessmer - -0.8.24 (2024-04-18) -------------------- - -0.8.23 (2024-02-20) -------------------- -* Merge pull request `#279 `_ from fmessmer/fix/emulation - reliable start -* reliable start -* Contributors: Felix Messmer, fmessmer - -0.8.22 (2023-04-29) -------------------- -* Merge pull request `#276 `_ from fmessmer/fix/emulation_odom_laser - publish odom_laser tf with current time -* publish odom_laser tf with current time -* Merge pull request `#275 `_ from FrederikPlahl/fix/odom_laser - Fix emulation bug in transform odom -> base_footprint -* remove obsolete import -* Change frequency -* Fix tf publishing bug -* Merge pull request `#274 `_ from HannesBachter/fix/rename_scan_odom - rename scan odom to /scan_odom_node/scan_odom/odometry -* rename scan odom to /scan_odom_node/scan_odom/odometry -* Contributors: Felix Messmer, FrederikPlahl, HannesBachter, fmessmer - -0.8.21 (2023-01-04) -------------------- - -0.8.20 (2022-11-17) -------------------- - -0.8.19 (2022-07-29) -------------------- - -0.8.18 (2022-01-12) -------------------- -* Merge pull request `#266 `_ from HannesBachter/fix/emulation - fix initialpose and add laser odom in emulation -* fix some typos -* add emulation_odom_laser to CMakeLists -* generate odom laser from odom (not from twist) -* publish odom->base in emulation_odom_laser -* add emulation for laser odom -* harmonize variables (add leading underscore) -* fix initialpose handling in emulation -* Contributors: Felix Messmer, HannesBachter - -0.8.17 (2021-12-23) -------------------- -* Merge pull request `#265 `_ from fmessmer/fix/emulation_argparse - fixup emulation argparse -* fixup emulation argparse -* Merge pull request `#264 `_ from HannesBachter/feature/laser_odometry - [no odom] make odom frame configurable -* make odom frame configurable -* Merge pull request `#263 `_ from fmessmer/emulation_nav - separate emulation_nav from emulation_base -* separate emulation_nav from emulation_base -* Contributors: Felix Messmer, HannesBachter, fmessmer - -0.8.16 (2021-10-19) -------------------- - -0.8.15 (2021-05-17) -------------------- -* Merge pull request `#257 `_ from fmessmer/fix_emulated_fjt_melodic - [melodic] fix emulated fjt -* never skip last traj point -* Contributors: Felix Messmer, fmessmer - -0.8.14 (2021-05-10) -------------------- - -0.8.13 (2021-04-06) -------------------- -* Merge pull request `#253 `_ from floweisshardt/feature/emulator_move_base_melodic - melodic/noetic: add optional move_base interface to base emulator -* add optional move_base interface to base emulator -* Merge pull request `#249 `_ from fmessmer/fix/fjt_emulation_melodic - [melodic] fix fjt emulation -* use timer for joint_states -* skip trajectory points faster than sample_rate -* introduce sample_rate_hz parameter -* Merge pull request `#247 `_ from fmessmer/fix_catkin_lint_melodic - [melodic] fix catkin_lint -* fix catkin_lint -* Contributors: Felix Messmer, Florian Weisshardt, floweisshardt, fmessmer - -0.8.12 (2020-10-21) -------------------- -* Merge pull request `#243 `_ from fmessmer/test_noetic - test noetic -* Bump CMake version to avoid CMP0048 warning -* Merge pull request `#244 `_ from lindemeier/bugfix/length-bug - [melodic] bugfix/length-bug -* sets correct length and checks for length -* Merge pull request `#240 `_ from benmaidel/feature/base_emu_reset_odom_melodic - reset odometry service for base emulation -* reset odometry service for base emulation -* Contributors: Benjamin Maidel, Felix Messmer, fmessmer, tsl - -0.8.11 (2020-03-21) -------------------- - -0.8.10 (2020-03-18) -------------------- -* Merge pull request `#236 `_ from fmessmer/melodic/emulation_clock_cpp - [melodic] add emulation_clock publisher - cpp variant -* fix boost timer + use dt_ms -* use boost timer -* configurable emulation dt -* add emulation_clock publisher - cpp variant -* Merge pull request `#234 `_ from fmessmer/emulation_custom_clock_melodic - [melodic] add emulation_clock publisher -* implement emulation_clock with timer callback -* add emulation_clock publisher -* Merge pull request `#228 `_ from fmessmer/feature/python3_compatibility_melodic - [ci_updates] pylint + Python3 compatibility - melodic -* fix pylint errors -* python3 compatibility via 2to3 -* Merge pull request `#226 `_ from fmessmer/ci_updates_melodic - [travis] ci updates - melodic -* catkin_lint fixes -* remove outdated script -* Contributors: Felix Messmer, Loy van Beek, fmessmer - -0.8.1 (2019-11-07) ------------------- -* Merge branch 'kinetic_dev' of github.com:ipa320/cob_control into melodic_dev -* add CHANGELOG for cob_hardware_emulation -* Merge pull request `#221 `_ from fmessmer/post_vacation_qa - [WIP] post vacation qa -* fix missing dependencies -* Merge pull request `#218 `_ from floweisshardt/fix/emulator - catch zero division if two trajectory points have the same time_from_start -* catch zero division if two trajectory points have the same time_from_start -* Merge pull request `#217 `_ from floweisshardt/emulator - initialpose from yaml file for base emulator -* initialpose from yaml file for base emulator -* Merge pull request `#216 `_ from benmaidel/feature/base_emulation_initialpose - add initialpose to emulation_base -* Merge pull request `#215 `_ from lindemeier/feature/1238-joint-trajectory-controller-emulator-linear-interpolation - Feature/1238 joint trajectory controller emulator linear interpolation -* 1238 Setting joint velocity and effort to zero after eaching final trajectory point -* reset odom on initialpose -* syntax fixes -* 1238 added service reset -* 1238 joint velocities added -* 1238 adding preempt polling - 1238 readme adjusted and small improvements -* add initialpose to emulation_base -* 1238 linear interpolation of joint states sampling the given trajectory - 1238 lerping start and goal works - 1238 Fixed error in lerp - 1238 using only local time segments for computing the interpolation weight - 1238 added more comments -* 1238 replacing timer with rospy loop rate - + publishing joint_states with 10Hz controlled by loop rate instead of timer -* Merge pull request `#214 `_ from floweisshardt/feature/emulator_base - emulator base can be used with real navigation -* migrate tf to tf2 -* emulator base can be used with real navigation -* Merge pull request `#212 `_ from floweisshardt/emulator - initial version of move_base emulator -* review comments -* initial version of move_base emulator -* Merge pull request `#211 `_ from ipa320/emulator - add hardware_emulation package -* add hardware_emulation package -* Contributors: Benjamin Maidel, Felix Messmer, Florian Weisshardt, Thomas Lindemeier, floweisshardt, fmessmer - -0.8.0 (2019-08-09) ------------------- - -0.7.8 (2019-08-09) ------------------- - -0.7.7 (2019-08-06) ------------------- - -0.7.6 (2019-06-07) ------------------- - -0.7.5 (2019-05-20) ------------------- - -0.7.4 (2019-04-05) ------------------- - -0.7.3 (2019-03-14) ------------------- - -0.7.2 (2018-07-21) ------------------- - -0.7.1 (2018-01-07) ------------------- - -0.7.0 (2017-07-18 10:50) ------------------------- - -0.6.15 (2017-07-18 10:30) -------------------------- - -0.6.14 (2016-10-10 12:20) -------------------------- - -0.6.13 (2016-10-10 11:46) -------------------------- - -0.6.12 (2016-10-10 11:45) -------------------------- - -0.6.11 (2016-04-01) -------------------- - -0.6.10 (2015-08-31) -------------------- - -0.6.9 (2015-08-25) ------------------- - -0.6.8 (2015-06-22) ------------------- - -0.6.7 (2015-06-17) ------------------- - -0.6.6 (2014-12-18 10:49) ------------------------- - -0.6.5 (2014-12-18 09:08) ------------------------- - -0.6.4 (2014-12-16 14:10) ------------------------- - -0.6.3 (2014-12-16 14:00) ------------------------- - -0.6.2 (2014-12-15) ------------------- - -0.6.1 (2014-09-22) ------------------- - -0.6.0 (2014-09-18) ------------------- - -0.5.4 (2014-08-26 10:26) ------------------------- - -0.1.0 (2014-08-26 10:23) ------------------------- diff --git a/cob_hardware_emulation/CMakeLists.txt b/cob_hardware_emulation/CMakeLists.txt deleted file mode 100644 index 9bedf0b4a..000000000 --- a/cob_hardware_emulation/CMakeLists.txt +++ /dev/null @@ -1,31 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(cob_hardware_emulation) - -find_package(catkin REQUIRED COMPONENTS roscpp rosgraph_msgs) -find_package(Boost REQUIRED COMPONENTS thread) - -catkin_package( - CATKIN_DEPENDS roscpp rosgraph_msgs - DEPENDS Boost -) - -include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) - -add_executable(emulation_clock src/emulation_clock.cpp) -add_dependencies(emulation_clock ${catkin_EXPORTED_TARGETS}) -target_link_libraries(emulation_clock ${catkin_LIBRARIES} ${Boost_LIBRARIES}) - -install(TARGETS emulation_clock - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -catkin_install_python(PROGRAMS - scripts/emulation_base.py - scripts/emulation_clock.py - scripts/emulation_follow_joint_trajectory.py - scripts/emulation_nav.py - scripts/emulation_odom_laser.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/cob_hardware_emulation/package.xml b/cob_hardware_emulation/package.xml deleted file mode 100644 index 926269320..000000000 --- a/cob_hardware_emulation/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - cob_hardware_emulation - 0.9.25 - The cob_hardware_emulation package provides idealized nodes emulating real robot hardware and/or physics simulation. - - Florian Weisshardt - Florian Weisshardt - - Apache 2.0 - - catkin - - roscpp - rosgraph_msgs - - actionlib - actionlib_msgs - control_msgs - move_base_msgs - nav_msgs - rospy - sensor_msgs - std_srvs - tf_conversions - tf2_ros - - diff --git a/cob_hardware_emulation/scripts/emulation_base.py b/cob_hardware_emulation/scripts/emulation_base.py deleted file mode 100755 index ca6ad6686..000000000 --- a/cob_hardware_emulation/scripts/emulation_base.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python - -import argparse -import copy -import math - -import rospy -import tf -import tf2_ros -from geometry_msgs.msg import Twist, TransformStamped, Pose -from nav_msgs.msg import Odometry -from std_srvs.srv import Trigger, TriggerRequest, TriggerResponse - -class EmulationBase(object): - def __init__(self, odom_frame): - # this node emulates the controllers of a base including twist_controller and odometry_controller - # - # interfaces - # - subscribers: - # - /base/twist_controller/command [geometry_msgs/Twist] - # - publishers: - # - /base/odometry_controller/odometry [nav_msgs/Odometry] - # - tf (odom_frame --> base_footprint) - - # TODO - # - speed factor - - self.odom_frame_ = odom_frame - - self.br = tf2_ros.TransformBroadcaster() - - self.timestamp_last_update = rospy.Time.now() - - self.twist = Twist() - self.timestamp_last_twist = rospy.Time(0) - - self.odom = Odometry() - self.odom.header.frame_id = self.odom_frame_ - self.odom.child_frame_id = "base_footprint" - self.odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion - - self.pub_odom = rospy.Publisher("/base/odometry_controller/odometry", Odometry, queue_size=1) - rospy.Subscriber("/base/twist_controller/command", Twist, self.twist_callback, queue_size=1) - rospy.Service("/base/odometry_controller/reset_odometry", Trigger, self.reset_odometry) - rospy.Timer(rospy.Duration(0.1), self.timer_cb) - - rospy.loginfo("Emulation for base running") - - def reset_odometry(self, req): - self.odom.pose.pose = Pose() - self.odom.pose.pose.orientation.w = 1 - - return TriggerResponse(True, "odometry resetted") - - def twist_callback(self, msg): - self.twist = msg - self.timestamp_last_twist = rospy.Time.now() - - def timer_cb(self, event): - # move robot (calculate new pose) - dt = rospy.Time.now() - self.timestamp_last_update - self.timestamp_last_update = rospy.Time.now() - time_since_last_twist = rospy.Time.now() - self.timestamp_last_twist - - #print "dt =", dt.to_sec(), ". duration since last twist =", time_since_last_twist.to_sec() - # we assume we're not moving any more if there is no new twist after 0.1 sec - if time_since_last_twist < rospy.Duration(0.1): - new_pose = copy.deepcopy(self.odom.pose.pose) - yaw = tf.transformations.euler_from_quaternion([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w])[2] + self.twist.angular.z * dt.to_sec() - quat = tf.transformations.quaternion_from_euler(0, 0, yaw) - new_pose.orientation.x = quat[0] - new_pose.orientation.y = quat[1] - new_pose.orientation.z = quat[2] - new_pose.orientation.w = quat[3] - new_pose.position.x += self.twist.linear.x * dt.to_sec() * math.cos(yaw) - self.twist.linear.y * dt.to_sec() * math.sin(yaw) - new_pose.position.y += self.twist.linear.x * dt.to_sec() * math.sin(yaw) + self.twist.linear.y * dt.to_sec() * math.cos(yaw) - self.odom.pose.pose = new_pose - - # we're moving, so we set a non-zero twist - self.odom.twist.twist.linear.x = self.twist.linear.x - self.odom.twist.twist.linear.y = self.twist.linear.y - self.odom.twist.twist.angular.z = self.twist.angular.z - else: - # reset twist as we're not moving anymore - self.odom.twist.twist = Twist() - - # publish odometry - odom = copy.deepcopy(self.odom) - odom.header.stamp = rospy.Time.now() # update to current time stamp - self.pub_odom.publish(odom) - - # publish tf - # pub base_footprint --> odom_frame - t_odom = TransformStamped() - t_odom.header.stamp = rospy.Time.now() - t_odom.header.frame_id = self.odom_frame_ - t_odom.child_frame_id = "base_footprint" - t_odom.transform.translation = self.odom.pose.pose.position - t_odom.transform.rotation = self.odom.pose.pose.orientation - - transforms = [t_odom] - - self.br.sendTransform(transforms) - -if __name__ == '__main__': - rospy.init_node('emulation_base') - parser = argparse.ArgumentParser(conflict_handler='resolve', - description="Tool for emulating base by publishing odometry and propagating base_footprint.") - parser.add_argument('-o', '--odom_frame', help='odom frame name (default: \'odom_combined\')', default='odom_combined') - args, unknown = parser.parse_known_args() - EmulationBase(args.odom_frame) - rospy.spin() diff --git a/cob_hardware_emulation/scripts/emulation_clock.py b/cob_hardware_emulation/scripts/emulation_clock.py deleted file mode 100755 index 40bd0d020..000000000 --- a/cob_hardware_emulation/scripts/emulation_clock.py +++ /dev/null @@ -1,28 +0,0 @@ -#!/usr/bin/env python - -import time -import rospy -from rosgraph_msgs.msg import Clock - -class EmulationClock(object): - def __init__(self): - self.t = time.time() - self.dt_ms = rospy.get_param('/emulation_dt_ms', 10) - self.time_factor = rospy.get_param('/emulation_time_factor', 1.0) - if not self.time_factor > 0.0: - rospy.logerr("emulation_time_factor must be >0.0, but is {}. exiting...".format(self.time_factor)) - exit(-1) - self.pub = rospy.Publisher('/clock', Clock, queue_size=1) - rospy.Timer(rospy.Duration(self.dt_ms/1000.0), self.timer_cb) - - def timer_cb(self, event): - self.t+=self.time_factor*self.dt_ms/1000 - msg = Clock() - msg.clock = rospy.Time(self.t) - self.pub.publish(msg) - - -if __name__ == '__main__': - rospy.init_node('emulation_clock', disable_rostime=True) - EmulationClock() - rospy.spin() diff --git a/cob_hardware_emulation/scripts/emulation_follow_joint_trajectory.py b/cob_hardware_emulation/scripts/emulation_follow_joint_trajectory.py deleted file mode 100755 index acdb9b424..000000000 --- a/cob_hardware_emulation/scripts/emulation_follow_joint_trajectory.py +++ /dev/null @@ -1,160 +0,0 @@ -#!/usr/bin/env python - -import copy - -import rospy -import actionlib -from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryResult -from sensor_msgs.msg import JointState -from std_srvs.srv import Trigger, TriggerResponse - -class EmulationFollowJointTrajectory(object): - def __init__(self): - # TODO - # - speed factor - - params = rospy.get_param('~') - self.joint_names = params['joint_names'] - # fixed frequency to control the granularity of the sampling resolution - self.sample_rate_hz = rospy.get_param("~sample_rate_hz", 10) - # duration from one sample to the next - self.sample_rate_dur_secs = (1.0 / float(self.sample_rate_hz)) - # rospy loop rate - self.sample_rate = rospy.Rate(self.sample_rate_hz) - - # joint_states publisher - js = JointState() - js.name = copy.deepcopy(self.joint_names) - js.position = [0]*len(js.name) - js.velocity = [0]*len(js.name) - js.effort = [0]*len(js.name) - self.joint_states = js - self.pub_joint_states = rospy.Publisher("joint_states", JointState, queue_size=1) - self.js_timer = rospy.Timer(rospy.Duration(self.sample_rate_dur_secs), self.publish_joint_states) - - # reset service - self.service_reset_fjta = rospy.Service("reset_joint_states", Trigger, self.reset_cb) - - # follow_joint_trajectory action - action_name = "joint_trajectory_controller/follow_joint_trajectory" - self.as_fjta = actionlib.SimpleActionServer(action_name, FollowJointTrajectoryAction, execute_cb=self.fjta_cb, auto_start = False) - self.as_fjta.start() - rospy.loginfo("Emulation running for action %s of type FollowJointTrajectoryAction"%(action_name)) - - def reset_cb(self, req): - self.joint_states.position = [0.0] * len(self.joint_states.position) - self.joint_states.velocity = [0.0] * len(self.joint_states.velocity) - self.joint_states.effort = [0.0] * len(self.joint_states.effort) - - return TriggerResponse( - success = True, - message = "Succesfully reset joint states" - ) - - def fjta_cb(self, goal): - joint_names = copy.deepcopy(self.joint_names) - joint_names.sort() - fjta_joint_names = copy.deepcopy(goal.trajectory.joint_names) - fjta_joint_names.sort() - if joint_names == fjta_joint_names: - rospy.loginfo("got a new joint trajectory goal for %s", joint_names) - # sort goal to fit joint_names order in joint_states - - goal_sorted = copy.deepcopy(goal) - goal_sorted.trajectory.joint_names = self.joint_names - - # keep track of the current time - latest_time_from_start = rospy.Duration(0) - # the point in time of a previous computation. - # This is used to compute the interpolation weight as a function of current and goal time point - time_since_start_of_previous_point = rospy.Duration(0) - - nr_points_dof = len(self.joint_names) - - # for all points in the desired trajectory - for p_idx, point in enumerate(goal_sorted.trajectory.points): - - if len(point.positions) != nr_points_dof: - self.as_fjta.set_aborted() - return - - point_time_delta = point.time_from_start - time_since_start_of_previous_point - if not p_idx == len(goal_sorted.trajectory.points)-1 and point_time_delta.to_sec() < self.sample_rate_dur_secs: - rospy.logwarn("current trajectory point has time_delta smaller than sample_rate: {} < {}! Skipping".format(point_time_delta.to_sec(), self.sample_rate_dur_secs)) - continue - - # we need to resort the positions array because moveit sorts alphabetically but all other ROS components sort in the URDF order - positions_sorted = [] - for joint_name in self.joint_names: - idx = goal.trajectory.joint_names.index(joint_name) - positions_sorted.append(point.positions[idx]) - point.positions = positions_sorted - - joint_states_prev = copy.deepcopy(self.joint_states) - - # linear interpolation of the given trajectory samples is used - # to compute smooth intermediate joints positions at a fixed resolution - - # upper bound of local duration segment - t1 = point.time_from_start - time_since_start_of_previous_point - # compute velocity as the fraction of distance from prev point to next point in trajectory - # and the corresponding time t1 - velocities = [0] * nr_points_dof - for i in range(nr_points_dof): - if t1.to_sec() != 0.0: - velocities[i] = (point.positions[i] - joint_states_prev.position[i]) / float(t1.to_sec()) - else: - velocities[i] = 0.0 - self.joint_states.velocity = velocities - - # this loop samples the time segment from the current states to the next goal state in "points" - while not rospy.is_shutdown() and ((point.time_from_start - latest_time_from_start) > rospy.Duration(0)): - # check that preempt has not been requested by the client - if self.as_fjta.is_preempt_requested(): - rospy.loginfo("preempt requested") - self.as_fjta.set_preempted() - return - - # current time passed in local duration segment - t0 = latest_time_from_start - time_since_start_of_previous_point - # compute the interpolation weight as a fraction of passed time and upper bound time in this local segment - if t1 != 0.0: - alpha = t0 / t1 - else: - alpha = 0.0 - - # interpolate linearly (lerp) each component - interpolated_positions = [0] * nr_points_dof - for i in range(nr_points_dof): - interpolated_positions[i] = (1.0 - alpha) * joint_states_prev.position[i] + alpha * point.positions[i] - self.joint_states.position = interpolated_positions - - # sleep until next sample update - self.sample_rate.sleep() - # increment passed time - latest_time_from_start += rospy.Duration(self.sample_rate_dur_secs) - - # ensure that the goal and time point is always exactly reached - latest_time_from_start = point.time_from_start - self.joint_states.position = point.positions - # set lower time bound for the next point - time_since_start_of_previous_point = latest_time_from_start - - # set joint velocities to zero after the robot stopped moving (reaching final point of trajectory) - self.joint_states.velocity = [0.0] * nr_points_dof - self.joint_states.effort = [0.0] * nr_points_dof - - self.as_fjta.set_succeeded(FollowJointTrajectoryResult()) - else: - rospy.logerr("received unexpected joint names in goal") - self.as_fjta.set_aborted() - - def publish_joint_states(self, event): - msg = copy.deepcopy(self.joint_states) - msg.header.stamp = rospy.Time.now() # update to current time stamp - self.pub_joint_states.publish(msg) - -if __name__ == '__main__': - rospy.init_node('emulation_follow_joint_trajectory') - emulation_follow_joint_trajectory = EmulationFollowJointTrajectory() - rospy.spin() \ No newline at end of file diff --git a/cob_hardware_emulation/scripts/emulation_nav.py b/cob_hardware_emulation/scripts/emulation_nav.py deleted file mode 100755 index f55a7cd2a..000000000 --- a/cob_hardware_emulation/scripts/emulation_nav.py +++ /dev/null @@ -1,168 +0,0 @@ -#!/usr/bin/env python - -import actionlib -import argparse -import numpy -import rospy -import tf -import tf2_ros -from actionlib_msgs.msg import GoalStatus -from geometry_msgs.msg import Transform, TransformStamped, PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped, Quaternion -from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal, MoveBaseResult - -class EmulationNav(object): - def __init__(self, odom_frame): - # this node emulates a very basic navigation including move_base action and localization - # - # interfaces - # - subscribers: - # - /initialpose [geometry_msgs/PoseWithCovarianceStamped] - # - publishers: - # - tf (map --> odom_frame) - # - actions: - # - move_base [move_base_msgs/MoveBaseAction] (optional) - - - # TODO - # - speed factor - # - handle cancel on move_base action - - self._odom_frame = odom_frame - - self._buffer = tf2_ros.Buffer() - self._listener = tf2_ros.TransformListener(self._buffer) - self._transform_broadcaster = tf2_ros.TransformBroadcaster() - - initialpose = rospy.get_param("~initialpose", None) - if type(initialpose) == list: - rospy.loginfo("using initialpose from parameter server: %s", str(initialpose)) - elif type(initialpose) == str: - rospy.loginfo("using initialpose from script server: %s", str(initialpose)) - initialpose = rospy.get_param("/script_server/base/" + initialpose) - else: - rospy.loginfo("initialpose not set, using [0, 0, 0] as initialpose") - initialpose = [0, 0, 0] - - self._odom_transform = Transform() - self._odom_transform.translation.x = initialpose[0] - self._odom_transform.translation.y = initialpose[1] - quat = tf.transformations.quaternion_from_euler(0, 0, initialpose[2]) - self._odom_transform.rotation = Quaternion(*quat) - - rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initalpose_callback, queue_size=1) - rospy.Timer(rospy.Duration(0.04), self.timer_cb) - - rospy.loginfo("Emulation for navigation running") - - # Optional move_base action - self._move_base_mode = rospy.get_param("~move_base_mode", None) - if self._move_base_mode == None: - rospy.loginfo("Emulation running without move_base") - elif self._move_base_mode == "beam" or self._move_base_mode == "linear_nav": - self._move_base_action_name = "/move_base" - rospy.Subscriber(self._move_base_action_name + "_simple/goal", PoseStamped, self.move_base_simple_callback, queue_size=1) - self._as_move_base = actionlib.SimpleActionServer(self._move_base_action_name, MoveBaseAction, execute_cb=self.move_base_cb, auto_start = False) - self._as_move_base.start() - rospy.loginfo("Emulation running for action %s of type MoveBaseAction with mode '%s'"%(self._move_base_action_name, self._move_base_mode)) - else: - rospy.logwarn("Emulation running without move_base due to invalid value for parameter move_base_mode: '%s'", self._move_base_mode) - - - def initalpose_callback(self, msg): - rospy.loginfo("Got initialpose, updating %s transformation.", self._odom_frame) - try: - # get pose of base_footprint within odom frame - base_in_odom = self._buffer.lookup_transform("base_footprint", self._odom_frame, rospy.Time(0)) - # convert initialpose to matrix (necessary for matrix multiplication) - trans_ini = tf.transformations.translation_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) - rot_ini = tf.transformations.quaternion_matrix([msg.pose.pose.orientation.x, msg.pose.pose.orientation.y, msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]) - matrix_ini = numpy.dot(trans_ini, rot_ini) - # convert footprint->odom transform to matrix (necessary for matrix multiplication) - trans_base_odom = tf.transformations.translation_matrix([base_in_odom.transform.translation.x, base_in_odom.transform.translation.y, base_in_odom.transform.translation.z]) - rot_base_odom = tf.transformations.quaternion_matrix([base_in_odom.transform.rotation.x, base_in_odom.transform.rotation.y, base_in_odom.transform.rotation.z, base_in_odom.transform.rotation.w]) - matrix_base_odom = numpy.dot(trans_base_odom, rot_base_odom) - - # matrix multiplication of footprint->odom transform in respect of initialpose - matrix_new_odom = numpy.dot(matrix_ini, matrix_base_odom) - - # translate back into Transform - trans_new_odom = tf.transformations.translation_from_matrix(matrix_new_odom) - rot_new_odom = tf.transformations.quaternion_from_matrix(matrix_new_odom) - self._odom_transform.translation.x = trans_new_odom[0] - self._odom_transform.translation.y = trans_new_odom[1] - self._odom_transform.translation.z = trans_new_odom[2] - self._odom_transform.rotation.x = rot_new_odom[0] - self._odom_transform.rotation.y = rot_new_odom[1] - self._odom_transform.rotation.z = rot_new_odom[2] - self._odom_transform.rotation.w = rot_new_odom[3] - - except Exception as e: - rospy.logerr("Could not calculate new odom transformation:\n%s", e) - return - - def timer_cb(self, event): - # publish tf - # pub odom_frame --> map - t_loc = TransformStamped() - t_loc.header.stamp = rospy.Time.now() - t_loc.header.frame_id = "map" - t_loc.child_frame_id = self._odom_frame - t_loc.transform = self._odom_transform - - transforms = [t_loc] - - self._transform_broadcaster.sendTransform(transforms) - - def move_base_cb(self, goal): - pwcs = PoseWithCovarianceStamped() - pwcs.header = goal.target_pose.header - pwcs.pose.pose = goal.target_pose.pose - if self._move_base_mode == "beam": - rospy.loginfo("move_base: beaming robot to new goal") - self.initalpose_callback(pwcs) - elif self._move_base_mode == "linear_nav": - move_base_linear_action_name = "/move_base_linear" - ac_move_base_linear = actionlib.SimpleActionClient(move_base_linear_action_name, MoveBaseAction) - rospy.loginfo("Waiting for ActionServer: %s", move_base_linear_action_name) - if not ac_move_base_linear.wait_for_server(rospy.Duration(1)): - rospy.logerr("Emulator move_base failed because move_base_linear action server is not available") - self._as_move_base.set_aborted() - return - rospy.loginfo("send goal to %s", move_base_linear_action_name) - ac_move_base_linear.send_goal(goal) - ac_move_base_linear.wait_for_result() - ac_move_base_linear_status = ac_move_base_linear.get_state() - ac_move_base_linear_result = ac_move_base_linear.get_result() - if ac_move_base_linear_status != GoalStatus.SUCCEEDED: - rospy.logerr("Emulator move_base failed because move_base_linear failed") - self._as_move_base.set_aborted() - return - else: - rospy.logerr("Invalid move_base_action_mode") - self._as_move_base.set_aborted() - return - rospy.loginfo("Emulator move_base succeeded") - self._as_move_base.set_succeeded(MoveBaseResult()) - - def move_base_simple_callback(self, msg): - goal = MoveBaseGoal() - goal.target_pose = msg - ac_move_base = actionlib.SimpleActionClient(self._move_base_action_name, MoveBaseAction) - rospy.loginfo("Waiting for ActionServer: %s", self._move_base_action_name) - if not ac_move_base.wait_for_server(rospy.Duration(1)): - rospy.logerr("Emulator move_base simple failed because move_base action server is not available") - return - rospy.loginfo("send goal to %s", self._move_base_action_name) - ac_move_base.send_goal(goal) - # ac_move_base.wait_for_result() # no need to wait for the result as this is the topic interface to move_base without feedback - -if __name__ == '__main__': - rospy.init_node('emulation_nav') - parser = argparse.ArgumentParser(conflict_handler='resolve', - description="Tool for emulating nav by providing localization and move base interface") - parser.add_argument('-o', '--odom_frame', help='odom frame name (default: \'odom_combined\')', default='odom_combined') - args, unknown = parser.parse_known_args() - rospy.loginfo("emulation_nav started!") - EmulationNav(args.odom_frame) - rospy.spin() diff --git a/cob_hardware_emulation/scripts/emulation_odom_laser.py b/cob_hardware_emulation/scripts/emulation_odom_laser.py deleted file mode 100755 index 0a9629500..000000000 --- a/cob_hardware_emulation/scripts/emulation_odom_laser.py +++ /dev/null @@ -1,65 +0,0 @@ -#!/usr/bin/env python - -import argparse -import rospy -import tf2_ros -from geometry_msgs.msg import TransformStamped -from nav_msgs.msg import Odometry - -class EmulationOdomLaser(object): - def __init__(self, odom_frame): - # this node emulates the laser odom based on the odometry from the base controller - # - # interfaces - # - subscribers: - # - /base/odometry_controller/odometry [nav_msgs/Odometry] - # - publishers: - # - /scan_odom_node/scan_odom/odometry [nav_msgs/Odometry] - # - tf (map --> odom_frame) - - - # TODO - # - speed factor - - self._odom_frame = odom_frame - - self._transform_broadcaster = tf2_ros.TransformBroadcaster() - - self._odom = Odometry() - self._odom.header.frame_id = self._odom_frame - self._odom.child_frame_id = "base_footprint" - self._odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion - - self._odom_publisher = rospy.Publisher("/scan_odom_node/scan_odom/odometry", Odometry, queue_size=1) - rospy.Subscriber("/base/odometry_controller/odometry", Odometry, self.odometry_callback, queue_size=1) - rospy.loginfo("Emulation for laser odometry running") - - def odometry_callback(self, msg): - self._odom = msg - self._odom.header.frame_id = self._odom_frame - self._odom_publisher.publish(self._odom) - self.publish_tf() - - def publish_tf(self): - # publish tf - # pub base_footprint --> odom_frame - t_odom = TransformStamped() - t_odom.header.stamp = rospy.Time.now() - t_odom.header.frame_id = self._odom_frame - t_odom.child_frame_id = "base_footprint" - t_odom.transform.translation = self._odom.pose.pose.position - t_odom.transform.rotation = self._odom.pose.pose.orientation - - transforms = [t_odom] - - self._transform_broadcaster.sendTransform(transforms) - -if __name__ == '__main__': - rospy.init_node('emulation_odom_laser') - parser = argparse.ArgumentParser(conflict_handler='resolve', - description="Tool for emulating laser odom based on the odometry from the base controller") - parser.add_argument('-o', '--odom_frame', help='odom frame name (default: \'odom_combined\')', default='odom_combined') - args, unknown = parser.parse_known_args() - rospy.loginfo("emulation_odom_laser running!") - EmulationOdomLaser(args.odom_frame) - rospy.spin() diff --git a/cob_hardware_emulation/src/emulation_clock.cpp b/cob_hardware_emulation/src/emulation_clock.cpp deleted file mode 100644 index c3de3114f..000000000 --- a/cob_hardware_emulation/src/emulation_clock.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include -#include -#include -#include - -#include -#include - - -class EmulationClock -{ -public: - EmulationClock(boost::asio::io_service& io, int dt_ms, double time_factor) - : dt_ms_(dt_ms), - time_factor_(time_factor), - timer_(io, boost::posix_time::milliseconds(dt_ms)) - { - t_ = (double)std::time(NULL); - pub_ = nh_.advertise("/clock", 1); - timer_.async_wait(boost::bind(&EmulationClock::timer_cb, this)); - } - - void timer_cb() - { - t_ += time_factor_*dt_ms_/1000; - ROS_DEBUG_STREAM("WALL: "<<(double)std::time(NULL)<<"; ROS: "<("/emulation_dt_ms", dt_ms, 10); - nh.param("/emulation_time_factor", time_factor, 1.0); - - boost::asio::io_service io; - EmulationClock ec(io, dt_ms, time_factor); - io.run(); - - return 0; -}