def prepare_grasp_server():
rospy.init_node('prepare_grasp_server')
s = rospy.service('prepare_grasp_service', PrepareGrasp,
handle_prepare_grasp)
# Keep the queue size as 1 for both topics
obj_to_point_sub = rospy.Subscriber('obj_to_point', ItemList,
obj_to_point_callback)
point_to_cloud_sub = rospy.Subscriber('point_to_cloud', ClusterList,
point_to_cloud_callback)
rospy.spin()
def __init__(self):
# Initialize node
rospy.init_node('simulation_node', anonymous=True)
# Create map listener
self.red_loc_sub = rospy.Subscriber('localization', RedRobotMap, self.update_red_robot_map)
self.green_loc_sub = rospy.Subscriber('localization', GreenRobotMap, self.update_green_robot_map)
#self.obstacle_loc_sub = rospy.Subscriber('localization', ObstacleRobotMap, self.update_obstacle_robot_map)
# Create publisher
self._red_pub = rospy.Publisher('/redbird/simulation/robots/red', RedRobotMap, queue_size=1)
self._green_pub = rospy.Publisher('/redbird/simulation/robots/green', GreenRobotMap, queue_size=1)
#self._obstacle_pub = rospy.Publisher('/redbird/simulation/robots/obstacle', ObstacleRobotMap, queue_size=1)
#create service
self._getting_coordinates_srv = rospy.service('/redbird/simulation/get_future_coords', GetFutureCoords, self.get_future_coords_handler)
# Create simulation object
self._sim = Simulation()
# Log waiting for ready flag
rospy.loginfo("Waiting for initial information from localization...")
# Wait for data to be populated from localization info
self._ready = False
while not self._ready:
pass
# Start simulation
self._sim.run()
# Log start
rospy.loginfo("Simulation started!")
# Creating maps
self.redrobotmap = RedRobotMap()
self.greenrobotmap = GreenRobotMap()
#self.obstaclerobotmap = ObstacleRobotMap()
# Create target robot list
self.red_robot_msgs = []
self.green_robot_msgs = []
#! /usr/bin/env python3
import board
import rospy
import busio
from adafruit_servokit import ServoKit
from arman_controller.srv import ArmanDriverRequest, ArmanDriverRequestResponse
i2c_addr = busio.I2C(board.SCL, board.SDA)
kit = ServoKit(channels=16, i2c=i2c_addr)
def update_joints(req):
kit.servo[0].angle = req.joint0
kit.servo[1].angle = req.joint1
kit.servo[2].angle = req.joint2
kit.servo[3].angle = req.joint3
kit.servo[4].angle = req.joint4
return ArmanDriverRequestResponse(1)
if __name__ == '__main__':
rospy.init_node('arman_driver')
s = rospy.service('arman_driver', ArmanDriverRequest, update_joints)
rospy.spin()
def __init__(self):
self.gripper_total_control_srv = rospy.service('gripper_total', gripper_total_control, self.gripper)
self.pub_9 = rospy.Publisher('/irl_dual_arm/joint9_position_controller/command', Float64)
self.pub_10 = rospy.Publisher('/irl_dual_arm/joint10_position_controller/command', Float64)
self.pub_11 = rospy.Publisher('/irl_dual_arm/joint11_position_controller/command', Float64)
print ("Ready to gripper_total_control.")
def republish_pointcloud_server():
rospy.init_node("pointcloud_republisher_server")
s = rospy.service('republish_pointcloud',RepublishPointcloud, republish_pointcloud)
rospy.spin()