def commandNavigateTo(posX, posY, channel):
goal = Position()
goal.x = posX
goal.y = posY
goal.z = 0
goalMessage = Message()
goalMessage.pack(goal)
goalMessage.topic = "RobotGateway.0.NavigateTo"
channel.publish(goalMessage)
def lemniscate_of_bernoulli(shape,
center,
lap_time,
rate=10.0,
allowed_error=0.1):
t = symbols('t')
w = 2 * np.pi / lap_time
phi = np.pi / 2
ax = (2 * shape[0]) / (3 - cos(2 * (w * t + phi)))
ay = (shape[1] / 0.35) / (3 - cos(2 * (w * t + phi)))
x = ax * cos((w * t + phi)) / 2 + center[0]
y = ay * sin(2 * (w * t + phi)) / 2 + center[1]
dx = diff(x, t)
dy = diff(y, t)
fx = lambdify((t), x, 'numpy')
fy = lambdify((t), y, 'numpy')
dfx = lambdify((t), dx, 'numpy')
dfy = lambdify((t), dy, 'numpy')
t = np.arange(0, lap_time, 1.0 / rate)
task = RobotTaskRequest()
task.basic_move_task.positions.extend(
[Position(x=x, y=y) for x, y in zip(fx(t), fy(t))])
task.basic_move_task.speeds.extend(
[Speed(linear=l, angular=w) for l, w in zip(dfx(t), dfy(t))])
task.basic_move_task.rate = rate
task.basic_move_task.allowed_error = allowed_error
return task
# get the transformation matrix corresponding to the current pose of the robot corrected when
# it sees an ArUco marker
robotToWorld = np.matrix(tensor.doubles).reshape(
tensor.shape.dims[0].size, tensor.shape.dims[1].size)
if goalWorldFrame.size > 0:
# matrix Inverse
toPepperFrame = inv(pepperPose)
# transform the goal from the world frame to the robot frame
goalPepperFrame = toPepperFrame * goalWorldFrame
posX = float(goalPepperFrame[0])
posY = float(goalPepperFrame[1])
posZ = float(goalPepperFrame[2])
if (True):
goal = Position()
goal.x = posX
goal.y = posY
goal.z = posZ
#print(goal.x,"#",goal.y,"#",goal.z)
goalMessage = Message()
goalMessage.pack(goal)
goalMessage.topic = "RobotGateway.0.NavigateTo"
else:
goal = Pose()
goal.position.x = posX
goal.position.y = posY
goal.position.z = posZ
goal.orientation.yaw = 90 * 3.14 / 180
goal.orientation.pitch = 0
goal.orientation.roll = 0
from is_wire.core import Channel, Subscription, Message, Logger
from is_msgs.common_pb2 import Position
import os
from sys import argv, exit
log = Logger(name="client")
uri = os.environ[
"BROKER_URI"] if "BROKER_URI" in os.environ else "amqp://10.10.2.20:30000"
log.info("uri={}", uri)
if len(argv) != 3:
log.error("Usage: python navigate.py <X> <Y>")
exit(-1)
channel = Channel(uri)
subscription = Subscription(channel)
position = Position()
position.x = float(argv[1])
position.y = float(argv[2])
channel.publish(message=Message(content=position, reply_to=subscription),
topic="RobotGateway.0.NavigateTo")
reply = channel.consume(timeout=1.0)
log.info("status={}", reply.status)
from is_wire.core import Channel, Logger
from src.utils.is_wire import RequestManager
from is_msgs.common_pb2 import Pose, Position
log = Logger(name='Client')
channel = Channel('amqp://localhost:5672')
request_manager = RequestManager(channel,
min_requests=15,
max_requests=30,
log_level=Logger.INFO)
poses = [(Pose(position=Position(x=i, y=2 * i)), i) for i in range(100)]
while True:
while request_manager.can_request() and len(poses) > 0:
pose, pose_id = poses.pop()
request_manager.request(content=pose,
topic="GetPosition",
timeout_ms=1000,
metadata=pose_id)
log.info('{:>6s} msg_id={}', " >>", pose_id)
received_msgs = request_manager.consume_ready(timeout=1.0)
for msg, metadata in received_msgs:
position = msg.unpack(Position)
log.info('{:<6s} msg_id={}', "<< ", metadata)
def final_position(target, rate=10, allowed_error=0.1):
task = RobotTaskRequest()
task.basic_move_task.positions.extend([Position(x=target[0], y=target[1])])
task.basic_move_task.rate = rate
task.basic_move_task.allowed_error = allowed_error
return task