Ejemplo n.º 1
0
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")
Ejemplo n.º 2
0
    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)
Ejemplo n.º 3
0
    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)
Ejemplo n.º 4
0
 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)
Ejemplo n.º 5
0
    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")
Ejemplo n.º 6
0
 def id2(self):
     send = commond()
     send.node = 4
     send.msg.id = 1
     self.pub_video.publish(send)
     rospy.loginfo("publish id2")
Ejemplo n.º 7
0
    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")