def test_2_manager(self):
req = SwitchController.Request()
req.strictness = SwitchController.Request.BEST_EFFORT
req.start_controllers = ["scaled_joint_trajectory_controller"]
result = self.call_service(self.switch_controller_client, req)
self.assertEqual(result.ok, True)
def switch_controllers(node, controller_manager_name, stop_controllers,
start_controllers, strict, start_asap, timeout):
request = SwitchController.Request()
request.start_controllers = start_controllers
request.stop_controllers = stop_controllers
if strict:
request.strictness = SwitchController.Request.STRICT
else:
request.strictness = SwitchController.Request.BEST_EFFORT
request.start_asap = start_asap
request.timeout = rclpy.duration.Duration(seconds=timeout).to_msg()
return service_caller(node, '{}/switch_controller'.format(controller_manager_name),
SwitchController, request)
def respawnModel(self):
print('Inicializando Respawn model')
service = '/gazebo/delete_model'
rospy.wait_for_service(service)
deleteModel = rospy.ServiceProxy(service, DeleteModel)
deleteModel(model_name='pioneer2dx')
os.system("roslaunch pioneer2dx respawn.launch")
print('configurando controlador')
service = '/rrbot/controller_manager/load_controller'
rospy.wait_for_service(service)
loadController = rospy.ServiceProxy(service, LoadController)
msg = LoadController._request_class()
msg.name = 'mobile_base_controller'
loadController(msg)
service = '/rrbot/controller_manager/switch_controller'
rospy.wait_for_service(service)
switchController = rospy.ServiceProxy(service, SwitchController)
msg = SwitchController._request_class()
msg.start_controllers = ['mobile_base_controller']
switchController(msg)
print('Respawn Finalizado')
def draw_circle():
# Initialize node
rospy.init_node('draw_circle_node')
# while rospy.get_rostime().to_sec() == 0.0:
# time.sleep(0.1)
# print(rospy.get_rostime().to_sec())
# Initiate publisher
trajectory_pub = rospy.Publisher('/planar_robot/joint_trajectory_controller/command',
JointTrajectory, queue_size=5)
joint_1_pub = rospy.Publisher('/planar_robot/joint_1_position_controller/command',
Float64, queue_size=1)
joint_2_pub = rospy.Publisher('/planar_robot/joint_2_position_controller/command',
Float64, queue_size=1)
# Set up the service
rospy.wait_for_service('/planar_robot/controller_manager/switch_controller')
# Ensure publisher is connected
rate = rospy.Rate(1)
while trajectory_pub.get_num_connections() == 0 or joint_1_pub.get_num_connections() == 0 or joint_2_pub.get_num_connections() == 0:
rate.sleep()
# Create instances of Float64 for joint command
j1 = Float64()
j2 = Float64()
# Define circle parameters
n = 100
dt = 0.01
radius = 0.1
circle_period = 4
duration = 6
elbow = 1 # elbow down
origin = [0.2, 0.2]
# Calculate first point on the circle
q, X, A = circle_IK(origin, radius, circle_period, 0, elbow)
j1.data = q[0]
j2.data = q[1]
# Publish the commands
# time.sleep(5)
rospy.loginfo("<<< STATUS: Moving to Start Position >>>")
joint_1_pub.publish(j1)
joint_2_pub.publish(j2)
time.sleep(4)
# Switch controller
try:
switch_controller = rospy.ServiceProxy(
'/planar_robot/controller_manager/switch_controller', SwitchController)
switch = SwitchController()
switch.start_controllers = ['joint_trajectory_controller']
switch.stop_controllers = ['joint_1_position_controller', 'joint_2_position_controller']
switch.strictness = 2
switch_controller.call(start_controllers=['joint_trajectory_controller'],
stop_controllers=['joint_1_position_controller', 'joint_2_position_controller'],
strictness=2)
except rospy.ServiceException, e:
print "Service call failed: %s"%e