浏览代码

Added M prefixes to docs (#167)

/merge-resolve-conflicts
peifeng-unity 3 年前
当前提交
ba9e559b
共有 4 个文件被更改,包括 66 次插入66 次删除
  1. 62
      tutorials/pick_and_place/2_ros_tcp.md
  2. 30
      tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs
  3. 4
      tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs
  4. 36
      tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs

62
tutorials/pick_and_place/2_ros_tcp.md


## The Unity Side
1. If you have not already completed the steps in [Part 0](0_ros_setup.md) to set up your ROS workspace and [Part 1](1_urdf.md) to set up the Unity project, do so now.
1. If you have not already completed the steps in [Part 0](0_ros_setup.md) to set up your ROS workspace and [Part 1](1_urdf.md) to set up the Unity project, do so now.
1. If the PickAndPlaceProject Unity project is not already open, select and open it from the Unity Hub.

![](img/2_menu.png)
In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory.
In the ROS Message Browser window, click `Browse` next to the ROS message path. Navigate to and select the ROS directory of this cloned repository (`Unity-Robotics-Hub/tutorials/pick_and_place/ROS/`). This window will populate with all msg and srv files found in this directory.
1. Next, the custom message scripts for this tutorial will need to be generated.
1. Next, the custom message scripts for this tutorial will need to be generated.
1. Finally, now that the messages have been generated, we will create the service for moving the robot.
1. Finally, now that the messages have been generated, we will create the service for moving the robot.
Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/srv` to view the srv file listed. Next to srv, click `Build 1 srv`.
Still in the ROS Message Browser window, expand `ROS/src/niryo_moveit/srv` to view the srv file listed. Next to srv, click `Build 1 srv`.
- Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories.
- Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MMoverServiceRequest and MMoverServiceResponse. These files describe the expected input and output formats for the service requests and responses when calculating trajectories.
> MessageGeneration generates two C# classes, a request and response, from a ROS srv file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Services](https://wiki.ros.org/Services).
You can now close the ROS Message Browser window.

}
```
> This function first takes in the current joint target values. Then, it grabs the poses of the `target` and the `targetPlacement` objects, adds them to the newly created message `sourceDestinationMessage`, and calls `Send()` to send this information to the ROS topic `topicName` (defined as `"SourceDestination_input"`).
> This function first takes in the current joint target values. Then, it grabs the poses of the `target` and the `targetPlacement` objects, adds them to the newly created message `sourceDestinationMessage`, and calls `Send()` to send this information to the ROS topic `topicName` (defined as `"SourceDestination_input"`).
1. Return to the Unity Editor. Now that the message contents have been defined and the publisher script added, it needs to be added to the Unity world to run its functionality.
1. Return to the Unity Editor. Now that the message contents have been defined and the publisher script added, it needs to be added to the Unity world to run its functionality.
Right click in the Hierarchy window and select "Create Empty" to add a new empty GameObject. Name it `Publisher`. Add the newly created SourceDestinationPublisher component to the Publisher GameObject by selecting the Publisher object. Click "Add Component" in the Inspector, and begin typing "SourceDestinationPublisher." Select the component when it appears.
Right click in the Hierarchy window and select "Create Empty" to add a new empty GameObject. Name it `Publisher`. Add the newly created SourceDestinationPublisher component to the Publisher GameObject by selecting the Publisher object. Click "Add Component" in the Inspector, and begin typing "SourceDestinationPublisher." Select the component when it appears.
1. Note that this component shows empty member variables in the Inspector window, which need to be assigned.
1. Note that this component shows empty member variables in the Inspector window, which need to be assigned.
1. Next, the ROS TCP connection needs to be created. Select `Robotics -> ROS Settings` from the top menu bar.
1. Next, the ROS TCP connection needs to be created. Select `Robotics -> ROS Settings` from the top menu bar.
In the ROS Settings window, the `ROS IP Address` should be the IP address of your ROS machine (*not* the one running Unity).

- If you **are** running ROS services in a Docker container, fill `ROS IP Address` with the loopback IP address `127.0.0.1` and the `Override Unity IP Address` as your local machine's IP address. Otherwise, leave the `Override Unity IP Address` field empty.
- If you **are** running ROS services in a Docker container, fill `ROS IP Address` and `Override Unity IP Address` with the loopback IP address `127.0.0.1`. Otherwise, leave the `Override Unity IP Address` field empty.
1. Next, we will add a UI element that will allow user input to trigger the `Publish()` function. In the Hierarchy window, right click to add a new UI > Button. Note that this will create a new Canvas parent as well.
1. Next, we will add a UI element that will allow user input to trigger the `Publish()` function. In the Hierarchy window, right click to add a new UI > Button. Note that this will create a new Canvas parent as well.
> Note: In case the Button does not start in the bottom left, it can be moved by setting the `Pos X` and `Pos Y` values in its Rect Transform component. For example, setting its Position to `(-200, -200, 0)` would set its position to the bottom right area of the screen.
> Note: In case the Button does not start in the bottom left, it can be moved by setting the `Pos X` and `Pos Y` values in its Rect Transform component. For example, setting its Position to `(-200, -200, 0)` would set its position to the bottom right area of the screen.
1. Select the newly made Button object, and scroll to see the Button component in the Inspector. Click the `+` button under the empty `OnClick()` header to add a new event. Select the `Publisher` object in the Hierarchy window and drag it into the new OnClick() event, where it says `None (Object)`. Click the dropdown where it says `No Function`. Select SourceDestinationPublisher > `Publish()`.
![](img/2_onclick.png)

roslaunch niryo_moveit part_2.launch
```
> Note: Running `roslaunch` automatically starts [ROS Core](http://wiki.ros.org/roscore) if it is not already running.
> Note: Running `roslaunch` automatically starts [ROS Core](http://wiki.ros.org/roscore) if it is not already running.
> Note: This launch file has been copied below for reference. The server_endpoint and trajectory_subscriber nodes are launched from this file, and the ROS params (set up in [Part 0](0_ros_setup.md)) are loaded from this command. The launch files for this project are available in the package's `launch` directory, i.e. `src/niryo_moveit/launch/`.

</launch>
```
This launch will print various messages to the console, including the set parameters and the nodes launched.
This launch will print various messages to the console, including the set parameters and the nodes launched.
ROS and Unity have now successfully connected!
![](img/2_echo.png)

- If the error `[rosrun] Found the following, but they're either not files, or not executable: server_endpoint.py` appears, the Python script may need to be marked as executable via `chmod +x Unity-Robotics-Hub/tutorials/pick_and_place/ROS/src/niryo_moveit/scripts/server_endpoint.py`.
- `...failed because unknown error handler name 'rosmsg'` This is due to a bug in an outdated package version. Try running `sudo apt-get update && sudo apt-get upgrade` to upgrade.
- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the ROS IP Address in the RosConnect in Unity, and that the `src/niryo_moveit/config/params.yaml` values are set correctly.
- If Unity fails to find a network connection, ensure that the ROS IP address is entered correctly as the ROS IP Address in the RosConnect in Unity, and that the `src/niryo_moveit/config/params.yaml` values are set correctly.
- If the ROS TCP handshake fails (e.g. `ROS-Unity server listening...` printed on the Unity side but no `ROS-Unity Handshake received` on the ROS side), the ROS IP may not have been set correctly in the params.yaml file. Try running `echo "ROS_IP: $(hostname -I)" > src/niryo_moveit/config/params.yaml` in a terminal from your ROS workspace.

- [TCP Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) package
- [Niryo One ROS stack](https://github.com/NiryoRobotics/niryo_one_ros)
- [MoveIt Msgs](https://github.com/ros-planning/moveit_msgs)
#### Proceed to [Part 3](3_pick_and_place.md).
#### Proceed to [Part 3](3_pick_and_place.md).

30
tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs


private readonly float jointAssignmentWait = 0.1f;
private readonly float poseAssignmentWait = 0.5f;
private readonly Vector3 pickPoseOffset = Vector3.up * 0.1f;
// Assures that the gripper is always positioned above the target cube before grasping.
private readonly Quaternion pickOrientation = Quaternion.Euler(90, 90, 0);

PickUp,
Place
};
/// <summary>
/// Close the gripper
/// </summary>

MNiryoMoveitJoints CurrentJointConfig()
{
MNiryoMoveitJoints joints = new MNiryoMoveitJoints();
joints.joint_00 = jointArticulationBodies[0].xDrive.target;
joints.joint_01 = jointArticulationBodies[1].xDrive.target;
joints.joint_02 = jointArticulationBodies[2].xDrive.target;

{
MMoverServiceRequest request = new MMoverServiceRequest();
request.joints_input = CurrentJointConfig();
// Pick Pose
request.pick_pose = new MPose
{

if (response.trajectories != null)
{
// For every trajectory plan returned
for (int poseIndex = 0; poseIndex < response.trajectories.Length; poseIndex++)
for (int poseIndex = 0 ; poseIndex < response.trajectories.Length; poseIndex++)
for (int jointConfigIndex = 0; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++)
for (int jointConfigIndex = 0 ; jointConfigIndex < response.trajectories[poseIndex].joint_trajectory.points.Length; jointConfigIndex++)
float[] result = jointPositions.Select(r => (float)r * Mathf.Rad2Deg).ToArray();
float[] result = jointPositions.Select(r=> (float)r * Mathf.Rad2Deg).ToArray();
var joint1XDrive = jointArticulationBodies[joint].xDrive;
var joint1XDrive = jointArticulationBodies[joint].xDrive;
joint1XDrive.target = result[joint];
jointArticulationBodies[joint].xDrive = joint1XDrive;
}

// Close the gripper if completed executing the trajectory for the Grasp pose
if (poseIndex == (int)Poses.Grasp)
CloseGripper();
// Wait for the robot to achieve the final pose from joint assignment
yield return new WaitForSeconds(poseAssignmentWait);
}

string arm_link = shoulder_link + "/arm_link";
jointArticulationBodies[1] = niryoOne.transform.Find(arm_link).GetComponent<ArticulationBody>();
string hand_link = wrist_link + "/hand_link";
jointArticulationBodies[5] = niryoOne.transform.Find(hand_link).GetComponent<ArticulationBody>();

rightGripper = rightGripperGameObject.GetComponent<ArticulationBody>();
leftGripper = leftGripperGameObject.GetComponent<ArticulationBody>();
}
}
}

4
tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs


// Send message to ROS and return the response
ros.SendServiceMessage<MPositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
awaitingResponseUntilTimestamp = Time.time + 1.0f; // don't send again for 1 second, or until we receive a response
awaitingResponseUntilTimestamp = Time.time+1.0f; // don't send again for 1 second, or until we receive a response
}
}

destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z);
Debug.Log("New Destination: " + destination);
}
}
}

36
tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs


using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor;
public class RosSubscriberExample : MonoBehaviour
{
public GameObject cube;
void Start()
{
ROSConnection.instance.Subscribe<RosColor>("color", ColorChange);
}
void ColorChange(RosColor colorMessage)
{
cube.GetComponent<Renderer>().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a);
}
}
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor;
public class RosSubscriberExample : MonoBehaviour
{
public GameObject cube;
void Start()
{
ROSConnection.instance.Subscribe<RosColor>("color", ColorChange);
}
void ColorChange(RosColor colorMessage)
{
cube.GetComponent<Renderer>().material.color = new Color32((byte)colorMessage.r, (byte)colorMessage.g, (byte)colorMessage.b, (byte)colorMessage.a);
}
}
正在加载...
取消
保存