浏览代码

Added M prefixes to docs (#167)

/laurie-RosConnection2.0
GitHub 4 年前
当前提交
b717fa8a
共有 11 个文件被更改,包括 45 次插入44 次删除
  1. 12
      tutorials/pick_and_place/2_ros_tcp.md
  2. 12
      tutorials/pick_and_place/3_pick_and_place.md
  3. 7
      tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs
  4. 18
      tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs
  5. 18
      tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs
  6. 2
      tutorials/ros_unity_integration/publisher.md
  7. 6
      tutorials/ros_unity_integration/service.md
  8. 2
      tutorials/ros_unity_integration/subscriber.md
  9. 2
      tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs
  10. 8
      tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs
  11. 2
      tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs

12
tutorials/pick_and_place/2_ros_tcp.md


![](img/2_robottraj.png)
- One new C# script should populate the `Assets/RosMessages/Moveit/msg` directory: RobotTrajectory.
- One new C# script should populate the `Assets/RosMessages/Moveit/msg` directory: MRobotTrajectory. This name is the same as the message you built, with an "M" prefix (for message).
1. Next, the custom message scripts for this tutorial will need to be generated.

- Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/msg` directory: NiryoMoveitJoints and NiryoTrajectory. NiryoMoveitJoints describes a value for each joint in the Niryo arm as well as poses for the target object and target goal. NiryoTrajectory describes a list of RobotTrajectory values, which will hold the calculated trajectories for the pick-and-place task.
- Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/msg` directory: MNiryoMoveitJoints and MNiryoTrajectory. MNiryoMoveitJoints describes a value for each joint in the Niryo arm as well as poses for the target object and target goal. MNiryoTrajectory describes a list of RobotTrajectory values, which will hold the calculated trajectories for the pick-and-place task.
> MessageGeneration generates a C# class from a ROS msg file with protections for use of C# reserved keywords and conversion to C# datatypes. Learn more about [ROS Messages](https://wiki.ros.org/Messages).

![](img/2_srv.png)
- Two new C# scripts should populate the `Assets/RosMessages/NiryoMoveit/srv` directory: MoverServiceRequest and MoverServiceResponse. 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).

```csharp
public void Publish()
{
NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints();
MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints();
sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;

sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;
// Pick Pose
sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose
sourceDestinationMessage.pick_pose = new MPose
{
position = target.transform.position.To<FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.

// Place Pose
sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose
sourceDestinationMessage.place_pose = new MPose
{
position = targetPlacement.transform.position.To<FLU>(),
orientation = pickOrientation.To<FLU>()

12
tutorials/pick_and_place/3_pick_and_place.md


```csharp
public void PublishJoints()
{
MoverServiceRequest request = new MoverServiceRequest();
MMoverServiceRequest request = new MMoverServiceRequest();
request.pick_pose = new RosMessageTypes.Geometry.Pose
request.pick_pose = new MPose
{
position = (target.transform.position + pickPoseOffset).To<FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.

// Place Pose
request.place_pose = new RosMessageTypes.Geometry.Pose
request.place_pose = new MPose
ros.SendServiceMessage<MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
ros.SendServiceMessage<MMoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
void TrajectoryResponse(MoverServiceResponse response)
void TrajectoryResponse(MMoverServiceResponse response)
{
if (response.trajectories != null)
{

> The `response.trajectories` are received in the `TrajectoryResponse()` callback, as defined in the `ros.SendServiceMessage` parameters. These trajectories are passed to `ExecuteTrajectories()` and executed as a [coroutine](https://docs.unity3d.com/Manual/Coroutines.html):
```csharp
private IEnumerator ExecuteTrajectories(MoverServiceResponse response)
private IEnumerator ExecuteTrajectories(MMoverServiceResponse response)
{
if (response.trajectories != null)
{

7
tutorials/pick_and_place/Scripts/SourceDestinationPublisher.cs


using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using RosMessageTypes.Geometry;
public class SourceDestinationPublisher : MonoBehaviour
{

public void Publish()
{
NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints();
MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints();
sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;

sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;
// Pick Pose
sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose
sourceDestinationMessage.pick_pose = new MPose
{
position = target.transform.position.To<FLU>(),
orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To<FLU>()

sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose
sourceDestinationMessage.place_pose = new MPose
{
position = targetPlacement.transform.position.To<FLU>(),
orientation = pickOrientation.To<FLU>()

18
tutorials/pick_and_place/Scripts/TrajectoryPlanner.cs


using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using RosMessageTypes.Geometry;
public class TrajectoryPlanner : MonoBehaviour
{

/// Get the current values of the robot's joint angles.
/// </summary>
/// <returns>NiryoMoveitJoints</returns>
NiryoMoveitJoints CurrentJointConfig()
MNiryoMoveitJoints CurrentJointConfig()
NiryoMoveitJoints joints = new NiryoMoveitJoints();
MNiryoMoveitJoints joints = new MNiryoMoveitJoints();
joints.joint_00 = jointArticulationBodies[0].xDrive.target;
joints.joint_01 = jointArticulationBodies[1].xDrive.target;

/// </summary>
public void PublishJoints()
{
MoverServiceRequest request = new MoverServiceRequest();
MMoverServiceRequest request = new MMoverServiceRequest();
request.pick_pose = new RosMessageTypes.Geometry.Pose
request.pick_pose = new MPose
{
position = (target.transform.position + pickPoseOffset).To<FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.

// Place Pose
request.place_pose = new RosMessageTypes.Geometry.Pose
request.place_pose = new MPose
ros.SendServiceMessage<MoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
ros.SendServiceMessage<MMoverServiceResponse>(rosServiceName, request, TrajectoryResponse);
void TrajectoryResponse(MoverServiceResponse response)
void TrajectoryResponse(MMoverServiceResponse response)
{
if (response.trajectories.Length > 0)
{

/// </summary>
/// <param name="response"> MoverServiceResponse received from niryo_moveit mover service running in ROS</param>
/// <returns></returns>
private IEnumerator ExecuteTrajectories(MoverServiceResponse response)
private IEnumerator ExecuteTrajectories(MMoverServiceResponse response)
{
if (response.trajectories != null)
{

18
tutorials/pick_and_place/Scripts_Part4/RealSimPickAndPlace.cs


using System.Collections;
using System.Linq;
using ROSGeometry;
using RosMessageTypes.Geometry;
using Unity.Robotics.ROSTCPConnector;
using Unity.Robotics.ROSTCPConnector.ROSGeometry;
using Pose = RosMessageTypes.Geometry.Pose;
using RosQuaternion = RosMessageTypes.Geometry.Quaternion;
public class RealSimPickAndPlace : MonoBehaviour
{

/// </summary>
public void PublishJoints()
{
MoverServiceRequest request = new MoverServiceRequest
MMoverServiceRequest request = new MMoverServiceRequest
{
joints_input =
{

joint_04 = jointArticulationBodies[4].xDrive.target,
joint_05 = jointArticulationBodies[5].xDrive.target
},
pick_pose = new Pose
pick_pose = new MPose
place_pose = new Pose
place_pose = new MPose
{
position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To<FLU>(),
orientation = pickOrientation.To<FLU>()

void Start()
{
ros.Subscribe<RobotMoveActionGoal>(rosRobotCommandsTopicName, ExecuteRobotCommands);
ros.Subscribe<MRobotMoveActionGoal>(rosRobotCommandsTopicName, ExecuteRobotCommands);
}
/// <summary>

/// </summary>
/// <param name="robotAction"> RobotMoveActionGoal of trajectory or gripper commands</param>
void ExecuteRobotCommands(RobotMoveActionGoal robotAction)
void ExecuteRobotCommands(MRobotMoveActionGoal robotAction)
{
if (robotAction.goal.cmd.cmd_type == TRAJECTORY_COMMAND_EXECUTION)
{

///
/// </summary>
/// <param name="trajectories"> The array of poses for the robot to execute</param>
private IEnumerator ExecuteTrajectories(RobotTrajectory trajectories)
private IEnumerator ExecuteTrajectories(MRobotTrajectory trajectories)
{
// For every robot pose in trajectory plan
foreach (var point in trajectories.joint_trajectory.points)

2
tutorials/ros_unity_integration/publisher.md


{
cube.transform.rotation = Random.rotation;
PosRot cubePos = new PosRot(
MPosRot cubePos = new MPosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,

6
tutorials/ros_unity_integration/service.md


cube.transform.rotation.w
);
PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos);
MPositionServiceRequest positionServiceRequest = new MPositionServiceRequest(cubePos);
ros.SendServiceMessage<PositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
ros.SendServiceMessage<MPositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
void Callback_Destination(PositionServiceResponse response)
void Callback_Destination(MPositionServiceResponse response)
{
awaitingResponseUntilTimestamp = -1;
destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z);

2
tutorials/ros_unity_integration/subscriber.md


```csharp
using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.RoboticsDemo.UnityColor;
using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor;
public class RosSubscriberExample : MonoBehaviour
{

2
tutorials/ros_unity_integration/unity_scripts/RosPublisherExample.cs


{
cube.transform.rotation = Random.rotation;
PosRot cubePos = new PosRot(
MPosRot cubePos = new MPosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,

8
tutorials/ros_unity_integration/unity_scripts/RosServiceExample.cs


{
Debug.Log("Destination reached.");
PosRot cubePos = new PosRot(
MPosRot cubePos = new MPosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,

cube.transform.rotation.w
);
PositionServiceRequest positionServiceRequest = new PositionServiceRequest(cubePos);
MPositionServiceRequest positionServiceRequest = new MPositionServiceRequest(cubePos);
ros.SendServiceMessage<PositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
ros.SendServiceMessage<MPositionServiceResponse>(serviceName, positionServiceRequest, Callback_Destination);
void Callback_Destination(PositionServiceResponse response)
void Callback_Destination(MPositionServiceResponse response)
{
awaitingResponseUntilTimestamp = -1;
destination = new Vector3(response.output.pos_x, response.output.pos_y, response.output.pos_z);

2
tutorials/ros_unity_integration/unity_scripts/RosSubscriberExample.cs


using UnityEngine;
using Unity.Robotics.ROSTCPConnector;
using RosColor = RosMessageTypes.RoboticsDemo.UnityColor;
using RosColor = RosMessageTypes.RoboticsDemo.MUnityColor;
public class RosSubscriberExample : MonoBehaviour
{

正在加载...
取消
保存