Browse Source

Add home/stow/stop_the_robot funcs to HelloNode

pull/106/head
Binit Shah 1 year ago
parent
commit
8626c4af36
1 changed files with 24 additions and 2 deletions
  1. +24
    -2
      hello_helpers/src/hello_helpers/hello_misc.py

+ 24
- 2
hello_helpers/src/hello_helpers/hello_misc.py View File

@ -164,6 +164,24 @@ class HelloNode:
continue continue
rate.sleep() rate.sleep()
def home_the_robot(self):
trigger_request = TriggerRequest()
trigger_result = self.home_the_robot_service(trigger_request)
rospy.logdebug(f"{self.node_name}'s HelloNode.home_the_robot: got message {trigger_result.message}")
return trigger_result.success
def stow_the_robot(self):
trigger_request = TriggerRequest()
trigger_result = self.stow_the_robot_service(trigger_request)
rospy.logdebug(f"{self.node_name}'s HelloNode.stow_the_robot: got message {trigger_result.message}")
return trigger_result.success
def stop_the_robot(self):
trigger_request = TriggerRequest()
trigger_result = self.stop_the_robot_service(trigger_request)
rospy.logdebug(f"{self.node_name}'s HelloNode.stop_the_robot: got message {trigger_result.message}")
return trigger_result.success
def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True): def main(self, node_name, node_topic_namespace, wait_for_first_pointcloud=True):
rospy.init_node(node_name) rospy.init_node(node_name)
self.node_name = rospy.get_name() self.node_name = rospy.get_name()
@ -181,10 +199,14 @@ class HelloNode:
self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback) self.point_cloud_subscriber = rospy.Subscriber('/camera/depth/color/points', PointCloud2, self.point_cloud_callback)
self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1) self.point_cloud_pub = rospy.Publisher('/' + node_topic_namespace + '/point_cloud2', PointCloud2, queue_size=1)
rospy.wait_for_service('/home_the_robot')
rospy.wait_for_service('/stow_the_robot')
rospy.wait_for_service('/stop_the_robot') rospy.wait_for_service('/stop_the_robot')
rospy.loginfo('Node ' + self.node_name + ' connected to /stop_the_robot service.')
rospy.loginfo('Node ' + self.node_name + ' connected to robot services.')
self.home_the_robot_service = rospy.ServiceProxy('/home_the_robot', Trigger)
self.stow_the_robot_service = rospy.ServiceProxy('/stow_the_robot', Trigger)
self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger) self.stop_the_robot_service = rospy.ServiceProxy('/stop_the_robot', Trigger)
if wait_for_first_pointcloud: if wait_for_first_pointcloud:
# Do not start until a point cloud has been received # Do not start until a point cloud has been received
point_cloud_msg = self.point_cloud point_cloud_msg = self.point_cloud

Loading…
Cancel
Save