Breaking News: Grepper is joining You.com. Read the official announcement!

MQTT_Subscriber

Karrtik Ma answered on July 27, 2024 Popularity 3/10 Helpfulness 1/10

answer MQTT_Subscriber

related MQTT_Subscriber

MQTT_Subscriber

Comment

using UnityEngine;
using System;
using System.Linq;
using uPLibrary.Networking.M2Mqtt;
using uPLibrary.Networking.M2Mqtt.Messages;
using Newtonsoft.Json;
using System.Collections.Concurrent;

public class MQTTSub : MonoBehaviour
{
    private MqttClient mqttClient;
    public string brokerAddress = "127.0.0.1";
    public int brokerPort = 1883;
    public string username = "karrtik";
    public string password = "123";
    public string[] topicsToSubscribe = new string[6] { "webots/joint1", "webots/joint2", "webots/joint3", "webots/joint4", "webots/joint5", "webots/joint6" };
    public string[] jointNames = new string[6] { "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" };
    public ArticulationBody[] jointArticulationBodies;

    private ConcurrentQueue<(int jointIndex, JointData data)> jointUpdateQueue = new ConcurrentQueue<(int, JointData)>();

    [Serializable]
    private class JointData
    {
        public float position;
    }

    void Start()
    {
        ConnectToMqttBroker();
    }

    void ConnectToMqttBroker()
    {
        try
        {
            Debug.Log($"Attempting to connect to MQTT broker at {brokerAddress}:{brokerPort}");
            mqttClient = new MqttClient(brokerAddress, brokerPort, false, null, null, MqttSslProtocols.None);
            mqttClient.MqttMsgPublishReceived += OnMqttMessageReceived;

            string clientId = Guid.NewGuid().ToString();
            mqttClient.Connect(clientId, username, password);

            if (mqttClient.IsConnected)
            {
                Debug.Log("Connected to MQTT broker");
                SubscribeToTopics();
            }
            else
            {
                Debug.LogError("Failed to connect to MQTT broker");
            }
        }
        catch (Exception e)
        {
            Debug.LogError($"Error connecting to MQTT broker: {e.Message}");
            Debug.LogError($"Stack trace: {e.StackTrace}");
        }
    }

    void SubscribeToTopics()
    {
        if (mqttClient.IsConnected)
        {
            mqttClient.Subscribe(topicsToSubscribe, Enumerable.Repeat(MqttMsgBase.QOS_LEVEL_AT_LEAST_ONCE, topicsToSubscribe.Length).ToArray());
            Debug.Log("Subscribed to topics");
        }
    }

    void OnMqttMessageReceived(object sender, MqttMsgPublishEventArgs e)
    {
        try
        {
            string topic = e.Topic;
            string message = System.Text.Encoding.UTF8.GetString(e.Message);
            JointData jointData = JsonConvert.DeserializeObject<JointData>(message);
            int jointIndex = Array.IndexOf(topicsToSubscribe, topic);
            if (jointIndex != -1)
            {
                jointUpdateQueue.Enqueue((jointIndex, jointData));
            }
            Debug.Log($"Received message on topic {topic}: Position = {jointData.position}");
        }
        catch (Exception ex)
        {
            Debug.LogError($"Error processing MQTT message: {ex.Message}");
        }
    }

    void FixedUpdate()
    {
        while (jointUpdateQueue.TryDequeue(out var jointUpdate))
        {
            UpdateJoint(jointUpdate.jointIndex, jointUpdate.data);
        }
    }

    void UpdateJoint(int jointIndex, JointData data)
    {
        if (jointIndex < jointArticulationBodies.Length)
        {
            ArticulationBody joint = jointArticulationBodies[jointIndex];
            ArticulationDrive drive = joint.xDrive;
            float targetRotation = data.position;

            // Invert rotation for all joints except shoulder_lift_joint
            if (jointNames[jointIndex] != "shoulder_lift_joint")
            {
                targetRotation = -targetRotation;
            }

            float currentRotation = drive.target;
            float newRotation = Mathf.Lerp(currentRotation, targetRotation * Mathf.Rad2Deg, Time.fixedDeltaTime * 10f);

            drive.target = newRotation;
            joint.xDrive = drive;
        }
    }

    void OnApplicationQuit()
    {
        if (mqttClient != null && mqttClient.IsConnected)
        {
            mqttClient.Disconnect();
        }
    }
}

Popularity 3/10 Helpfulness 1/10 Language csharp

Source: Grepper

Tags: c#

Link to this answer
Share Copy Link

Contributed on Jul 27 2024

Karrtik Ma

0 Answers Avg Quality 2/10

Closely Related Answers

MQTT_Subscriber

Comment

from controller import Robot
import paho.mqtt.client as mqtt
import json
import math
import time

# Initialize the robot
robot = Robot()
timestep = int(robot.getBasicTimeStep())

# Set up joints
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint"]
joints = []

for name in joint_names:
    joint = robot.getDevice(name)
    if joint:
        # Print current joint limits
        min_pos = joint.getMinPosition()
        max_pos = joint.getMaxPosition()
        print(f"{joint.getName()} limits: [{min_pos}, {max_pos}]")
        
        joint.setPosition(float('inf'))
        joint.setVelocity(0.0)
        joint.setAvailableTorque(100)  # Adjust this value as needed
        joint.setVelocity(joint.getMaxVelocity())
        
        # Enable position sensor
        sensor = joint.getPositionSensor()
        if sensor:
            sensor.enable(timestep)
        
        joints.append(joint)

# Function to move joints to home position
def move_to_home_position():
    print("Moving to home position...")
    for joint in joints:
        joint.setPosition(0)  # Set each joint to 0 radians (0 degrees)
    
    # Wait for joints to reach home position
    timeout = 5  # 5 seconds timeout
    start_time = time.time()
    while time.time() - start_time < timeout:
        all_at_home = True
        for joint in joints:
            current_pos = joint.getPositionSensor().getValue()
            if abs(current_pos) > 0.01:  # Allow small tolerance
                all_at_home = False
                break
        if all_at_home:
            break
        robot.step(timestep)
    
    print("Home position reached")

# Move to home position before starting MQTT communication
move_to_home_position()

# MQTT setup
broker_address = "127.0.0.1"
broker_port = 1883
topics = [f"unity/joint{i+1}" for i in range(len(joints))]

def on_connect(client, userdata, flags, rc):
    print(f"Connected with result code {rc}")
    for topic in topics:
        client.subscribe(topic)
        print(f"Subscribed to {topic}")

def on_message(client, userdata, message):
    try:
        topic = message.topic
        payload = json.loads(message.payload.decode())
        joint_index = topics.index(topic)
        
        rotation = math.radians(payload['rotation'])
        
        # Invert rotation for all joints except shoulder_lift_joint
        if joint_names[joint_index] != "shoulder_lift_joint":
            rotation = -rotation
        
        joints[joint_index].setPosition(rotation)
        
        print(f"Set joint {joint_names[joint_index]} to rotation {math.degrees(rotation):.2f} degrees")
    except Exception as e:
        print(f"Error processing message: {e}")

mqtt_client = mqtt.Client()
mqtt_client.on_connect = on_connect
mqtt_client.on_message = on_message

# Set username and password
mqtt_client.username_pw_set("karrtik", "123")

mqtt_client.connect(broker_address, broker_port, 60)
mqtt_client.loop_start()

# Main control loop
while robot.step(timestep) != -1:
    # Print current positions
    for i, joint in enumerate(joints):
        sensor = joint.getPositionSensor()
        if sensor:
            current_position = sensor.getValue()
            print(f"Joint {joint_names[i]} current position: {math.degrees(current_position):.2f} degrees")

mqtt_client.loop_stop()
mqtt_client.disconnect()

Popularity 3/10 Helpfulness 1/10 Language python

Source: Grepper

Tags: python

Link to this answer
Share Copy Link

Contributed on Jul 27 2024

Karrtik Ma

0 Answers Avg Quality 2/10

MQTT_Subscriber

Contents

More Related Answers

MQTT_Subscriber

Closely Related Answers

MQTT_Subscriber

Grepper

Documentation

Social

Legal

Contact

Oops, You will need to install Grepper and log-in to perform this action.