def on_message(client, userdata, msg):
msg_ = commond()
print(msg.topic + " " + str(msg.payload))
data = json.loads(msg.payload.decode("utf-8"))["data"]
if data == "beer":
msg_.node = 4
msg_.msg.id = 1
pub.publish(msg_)
rospy.loginfo("beer")
elif data == "drink":
msg_.node = 4
msg_.msg.id = 2
pub.publish(msg_)
rospy.loginfo("drink")
else:
rospy.loginfo("invalid voice message")
def callback(self, msg):
if ((msg.node == 3) or (msg.node == 0)):
send = commond()
send.topic_id = msg.topic_id
rospy.loginfo("%d", msg.topic_id)
if (msg.msg.id == 0): # grab can
self.pose_right_2()
self.pose_upper()
self.arm.grip(self.gripper_open)
self.pose_front()
self.pose_grip()
self.arm.grip(self.gripper_close)
self.pose_front()
self.pose_upper()
self.pose_right_2()
self.pose_right_1()
send.ret = 0
rospy.loginfo("grab can")
elif (msg.msg.id == 1): # release can
self.pose_right_2()
self.pose_upper()
self.pose_front()
self.pose_release()
self.arm.grip(self.gripper_open)
rospy.Rate(0.25).sleep() # sleep 4sec
self.pose_front()
self.arm.grip(self.gripper_close)
self.pose_right_2()
self.pose_right_1()
send.ret = 0
rospy.loginfo("release can")
else:
send.ret = 0
rospy.loginfo("undifined id")
self.pub_arm.publish(send)
else:
rospy.loginfo("this topic is not for arm, node=%d", msg.topic_id)
def callback(self, msg):
if((msg.node == 4)or(msg.node == 0)):
send = commond()
send.topic_id = msg.topic_id
rospy.loginfo("%d", msg.topic_id)
if(msg.msg.id == 0):
rospy.loginfo("waiting...")
time.sleep(20)
send.ret = 0
elif(msg.msg.id == 1):
print(self.filename)
totem = subprocess.Popen(['totem', '/home/ubuntu/catkin_ws/src/voice/scripts/cigarette_cm.mp4'])
time.sleep(40)
totem.kill()
send.ret = 0
rospy.loginfo("finish action")
self.pub_video.publish(send)
def callback(self, msg):
if((msg.node == 3)or(msg.node == 0)):
send = commond()
send.topic_id = msg.topic_id
rospy.loginfo("%d", msg.topic_id)
if (msg.msg.id == 0):
self.act.pregrab()
self.act.grab()
self.act.maintain()
send.ret = 0
rospy.loginfo("arm moved")
elif (msg.msg.id == 1):
self.act.release()
self.act.startpos()
send.ret = 0
else:
send.ret = 0
self.pub_arm.publish(send)
else:
rospy.loginfo("this topic is not for arm, node=%d", msg.topic_id)
def callback(self, msg):
if((msg.node == 2)or(msg.node == 0)):
send = commond()
send.topic_id = msg.topic_id
rospy.loginfo("%d", msg.topic_id)
if(msg.msg.id == 0): # random move
rospy.loginfo("Move without goal")
Adj = adjust.Adjust()
Adj.angular_speed = np.deg2rad(20)
Adj.rotate(np.deg2rad(20))
#print("ret:", ret)
self.cnt_angle += 1
"""
if(self.cnt_angle * np.deg2rad(30) == 2 * np.pi):
self.cnt_angle = 0
angle = np.random.rand() * 2 * np.pi
if(angle > np.pi):
angle = angle - 2 * np.pi
Adj.rotate(angle)
MC = move_goal.MoveCoordinate()
point_base = Point()
point_base.x = 0.3
point_base.y = 0
point_base.z = 0
pose_base = MC.calc_goal(point_base, dist=0)
pose_map = MC.transform_pose(pose_base,
MC.get_tf('map', 'base_link'))
ret = MC.move_pose(pose_map)
print("goal@base", pose_base)
print("-------------------------------------------")
print("goal@map", pose_map)
print("-------------------------------------------")
"""
send.ret = 0 # succeed
elif(msg.msg.id == 1): # move target
rospy.loginfo("Move with goal %f %f %f", msg.msg.x3d, msg.msg.y3d, msg.msg.z3d)
self.cnt_angle = 0
point_cam = Point()
point_cam.x = msg.msg.x3d
point_cam.y = msg.msg.y3d
point_cam.z = msg.msg.z3d
MC = move_goal.MoveCoordinate()
point_base = MC.transform_point(point_cam,
MC.get_tf('base_link', 'camera_link'))
pose_base = MC.calc_goal(point_base, dist=0.5)
pose_map = MC.transform_pose(pose_base,
MC.get_tf('map', 'base_link'))
MC.move_pose(pose_map)
print("target@cam", point_cam)
print("-------------------------------------------")
print("target@base", point_base)
print("-------------------------------------------")
print("goal@base", pose_base)
print("-------------------------------------------")
print("goal@map", pose_map)
print("-------------------------------------------")
send.ret = 0 # succeed
elif(msg.msg.id == 2): # adjust
rospy.loginfo("Adjust position")
point_cam = Point()
point_cam.x = msg.msg.x3d
point_cam.y = msg.msg.y3d
point_cam.z = msg.msg.z3d
MC = move_goal.MoveCoordinate()
point_base = MC.transform_point(point_cam,
MC.get_tf('base_link', 'camera_link'))
theta = np.arctan2(point_base.y, point_base.x)
Adj = adjust.Adjust()
Adj.angular_speed = np.deg2rad(20)
Adj.rotate(theta) # 回転
if(msg.msg.key == 0): # オブジェクトへの移動
distance = np.sqrt(point_base.x**2 + point_base.y**2) - 0.405
elif(msg.msg.key == 1): # 人への移動
distance = np.sqrt(point_base.x**2 + point_base.y**2) - 0.405
Adj.back_and_forward(distance) # 直進
print("target@cam", point_cam)
print("-------------------------------------------")
print("theta :%f", np.rad2deg(theta))
print("-------------------------------------------")
print("distance :%f", distance)
print("-------------------------------------------")
send.ret = 0 # succeed
elif(msg.msg.id == 3): # move origin
rospy.loginfo("Move origin")
MC = move_goal.MoveCoordinate()
# point_map.x = 0
# point_map.y = 0
# point_map.z = 0
point_map = self.point_map_0
point_base = MC.transform_point(point_map,
MC.get_tf('base_link', 'map'))
pose_base = MC.calc_goal(point_base, dist=0)
pose_map = MC.transform_pose(pose_base,
MC.get_tf('map', 'base_link'))
theta = np.arctan2(point_base.y, point_base.x)
Adj = adjust.Adjust()
Adj.angular_speed = np.deg2rad(45)
Adj.rotate(theta) # 移動方向に回転
MC.move_pose(pose_map) # 自律移動
print("goal@base ", pose_base)
print("-------------------------------------------")
print("goal@map ", pose_map)
print("-------------------------------------------")
print("theta :%f", np.rad2deg(theta))
print("-------------------------------------------")
send.ret = 0 # succeed
elif(msg.msg.id == 4): # turn towards object
rospy.loginfo("Turn towards target")
point_cam = Point()
point_cam.x = msg.msg.x3d
point_cam.y = msg.msg.y3d
point_cam.z = msg.msg.z3d
Adj = adjust.Adjust()
MC = move_goal.MoveCoordinate()
point_base = MC.transform_point(point_cam, MC.trans_cam2base)
theta = np.arctan2(point_base.y, point_base.x)
rospy.loginfo("turn %f degree", np.rad2deg(theta))
Adj.rotate(theta) # 回転
print("theta :%f", np.rad2deg(theta))
print("-------------------------------------------")
send.ret = 0
else: # invalid move mode
rospy.loginfo("invalid move mode @RULO")
self.pub_rulo.publish(send)
rospy.loginfo('published')
else:
rospy.loginfo("this topic is not for rulo")
def id2(self):
send = commond()
send.node = 4
send.msg.id = 1
self.pub_video.publish(send)
rospy.loginfo("publish id2")
def callback(self, msg):
if ((msg.node == 2) or (msg.node == 0)):
send = commond()
send.topic_id = msg.topic_id
rospy.loginfo("%d", msg.topic_id)
if (msg.msg.id == 0): #rondom
rospy.loginfo("moving without goal")
# simple version
angle = np.deg2rad(5)
liner = 0.2
mov = rondom_move.Moving()
mov.rotate(angle)
self.cnt_angle += 1
if self.cnt_angle == 12:
mov.liner_x(liner)
self.cnt_liner += 1
if self.cnt_liner == 3:
liner = -liner
send.ret = 0 # succeed
elif (msg.msg.id == 1): # move goal
rospy.loginfo("moving with goal %f %f %f", msg.msg.x3d,
msg.msg.y3d, msg.msg.z3d)
point = Point()
point.x = msg.msg.x3d
point.y = msg.msg.y3d
point.z = msg.msg.z3d
MC = move_goal.MoveCoordinate()
MC.cam_frame = 'camera_link'
MC.move_goal(point)
send.ret = 0 # succeed
elif (msg.msg.id == 2): # adjust
rospy.loginfo("position adjustment %d %d", msg.msg.x2d,
msg.msg.y2d)
point = Point()
point.x = msg.msg.x3d
point.y = msg.msg.y3d
point.z = msg.msg.z3d
cmd = adjust.Cmd()
if (msg.msg.key == 0):
cmd.goal_z = 0.18
elif (msg.msg.key == 1):
cmd.goal_z = 0.30
cmd.rotate(point)
cmd.back_and_forward(point)
send.ret = 0 # succeed
elif (msg.msg.id == 3): # move goal
rospy.loginfo("moving with goal %f %f %f", 0, 0, 0)
point = Point()
point.x = 0
point.y = 0
point.z = 0
MC = move_goal.MoveCoordinate()
MC.cam_frame = 'map'
MC.dist = 0
MC.obj_radius = 0
MC.move_goal(point)
send.ret = 0 # succeed
else: # invalid move mode
rospy.loginfo("invalid move mode @RULO")
self.pub_rulo.publish(send)
else:
rospy.loginfo("this topic is not for rulo")
