public void OnRosConnected() { if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (System.Action <Ros.VehicleCmd>)((Ros.VehicleCmd msg) => { lastUpdate = Time.time; var targetLinear = (float)msg.twist_cmd.twist.linear.x; var targetAngular = (float)msg.twist_cmd.twist.angular.z; if (!keyboard) { var linMag = Mathf.Abs(targetLinear - actualLinVel); if (actualLinVel < targetLinear && !controller.InReverse) { inputAccel = Mathf.Clamp(linMag, 0, constAccel); } else if (actualLinVel > targetLinear && !controller.InReverse) { inputAccel = -Mathf.Clamp(linMag, 0, constAccel); } targetAngVel = -Mathf.Clamp(targetAngular * 0.5f, -constSteer, constSteer); } })); } else if (TargetRosEnv == ROSTargetEnvironment.APOLLO) { Bridge.Subscribe <Ros.control_command>(APOLLO_CMD_TOPIC, (System.Action <Ros.control_command>)(msg => { lastUpdate = Time.time; var pedals = GetComponent <PedalInputController>(); throttle = pedals.throttleInputCurve.Evaluate((float)msg.throttle / 100); brake = pedals.brakeInputCurve.Evaluate((float)msg.brake / 100); var linearAccel = throttle - brake; var timeStamp = (float)msg.header.timestamp_sec; var steeringTarget = -((float)msg.steering_target) / 100; var dt = timeStamp - lastTimeStamp; lastTimeStamp = timeStamp; var sgn = Mathf.Sign(steeringTarget - steeringAngle); var steeringRate = (float)msg.steering_rate * sgn; steeringAngle += steeringRate * dt; // to prevent oversteering if (sgn != steeringTarget - steeringAngle) { steeringAngle = steeringTarget; } if (!keyboard) { targetAngVel = steeringAngle; inputAccel = linearAccel; } })); } }
public void OnRosConnected() { Bridge.Subscribe <Ros.control_command>(VehicleInputController.APOLLO_CMD_TOPIC, msg => { Wheel.autonomousBehavior = SteerWheelAutonomousFeedbackBehavior.OutputOnly; }); }
public void OnRosConnected() { Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (System.Action <Ros.VehicleCmd>)((Ros.VehicleCmd msg) => { var targetLinear = msg.twist_cmd.twist.linear; var targetAngular = msg.twist_cmd.twist.angular; if (!keyboard) { targetAngVel = (float)targetAngular.z; targetLinVel = (float)targetLinear.x; } })); }
public void OnRosConnected() { Bridge.Subscribe <Ros.Vector3>(ResetTopic, msg => { var position = GpsDevice.GetPosition(msg.x, msg.y); int mask = ~(1 << 8); // car layer RaycastHit hit; if (Physics.Raycast(position + new Vector3(0, 100, 0), new Vector3(0, -1, 0), out hit, Mathf.Infinity, mask)) { position = hit.point; position.y += 0.01f; } else { position.y += 20.0f; } var angle = (float)msg.z * Mathf.Rad2Deg - GpsDevice.Angle; var rotation = Quaternion.AngleAxis(angle, Vector3.up); transform.SetPositionAndRotation(position, rotation); }); }
public void OnRosConnected() { Bridge.Subscribe <Ros.Vector3>(ResetTopic, msg => { var position = GpsDevice.GetPosition(msg.x, msg.y); int mask = 1 << LayerMask.NameToLayer("Ground And Road"); RaycastHit hit; if (Physics.Raycast(position + new Vector3(0, 100, 0), new Vector3(0, -1, 0), out hit, Mathf.Infinity, mask)) { position = hit.point; position.y += 0.01f; } else { position.y += 20.0f; } var angle = (float)msg.z * Mathf.Rad2Deg - GpsDevice.Angle; var rotation = Quaternion.AngleAxis(angle, Vector3.up); // reset position, rotation, velocity and angular velocity GpsDevice.Agent.GetComponent <VehicleInputController>().controller.ResetSavedPosition(position, rotation); }); }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Detection3DArray>(objects3DTopicName); } if (targetEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.Subscribe <Ros.DetectedObjectArray>(autowareLidarDetectionTopicName, msg => { if (!isLidarPredictionEnabled || lidarPredictedObjects == null) { return; } foreach (Ros.DetectedObject obj in msg.objects) { Ros.Detection3D obj_converted = new Ros.Detection3D() { header = new Ros.Header() { stamp = new Ros.Time() { secs = obj.header.stamp.secs, nsecs = obj.header.stamp.nsecs, }, seq = obj.header.seq, frame_id = obj.header.frame_id, }, id = obj.id, label = obj.label, score = obj.score, bbox = new Ros.BoundingBox3D() { position = new Ros.Pose() { position = new Ros.Point() { x = obj.pose.position.x, y = obj.pose.position.y, z = obj.pose.position.z, }, orientation = new Ros.Quaternion() { x = obj.pose.orientation.x, y = obj.pose.orientation.y, z = obj.pose.orientation.z, w = obj.pose.orientation.w, }, }, size = new Ros.Vector3() { x = obj.dimensions.x, y = obj.dimensions.y, z = obj.dimensions.z, }, }, velocity = new Ros.Twist() { linear = new Ros.Vector3() { x = obj.velocity.linear.x, y = 0, z = 0, }, angular = new Ros.Vector3() { x = 0, y = 0, z = obj.velocity.angular.z, }, }, }; lidarPredictedObjects.Add(obj_converted); } lidarPredictedVisuals = lidarPredictedObjects.ToList(); lidarPredictedObjects.Clear(); }); } }
public void OnRosConnected() { if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (System.Action <Ros.VehicleCmd>)((Ros.VehicleCmd msg) => { lastAutoUpdate = Time.time; bool pub_ctrl_cmd = false; bool pub_gear_cmd = false; var gearCmd = msg.gear; if (gearCmd != 0) { pub_gear_cmd = true; } var ctrlCmd_linVel = msg.ctrl_cmd.linear_velocity; var ctrlCmd_linAcc = msg.ctrl_cmd.linear_acceleration; var ctrlCmd_steerAng = msg.ctrl_cmd.steering_angle; if (ctrlCmd_linAcc == 0 && ctrlCmd_linVel == 0 && ctrlCmd_steerAng == 0) { pub_ctrl_cmd = false; } else { pub_ctrl_cmd = true; } if (!pub_ctrl_cmd && !pub_gear_cmd) { // using twist_cmd to control ego vehicle var targetLinear = (float)msg.twist_cmd.twist.linear.x; var targetAngular = (float)msg.twist_cmd.twist.angular.z; if (!underKeyboardControl) { var linMag = Mathf.Abs(targetLinear - actualLinVel); if (actualLinVel < targetLinear && !controller.InReverse) { autoInputAccel = Mathf.Clamp(linMag, 0, constAccel); } else if (actualLinVel > targetLinear && !controller.InReverse) { autoInputAccel = -Mathf.Clamp(linMag, 0, constAccel); } autoSteerAngle = -Mathf.Clamp(targetAngular * 0.5f, -constSteer, constSteer); } } else { // using gear and ctrl_cmd to control ego vehicle if (gearCmd == 64) { controller.GearboxShiftDown(); } else // Switch to "Drive" for anything but "Reverse" { controller.GearboxShiftUp(); } if (!underKeyboardControl) { // ignoring the control linear velocity for now. autoSteerAngle = (float)ctrlCmd_steerAng; // angle should be in degrees autoInputAccel = Mathf.Clamp((float)ctrlCmd_linAcc, -1, 1); } } })); } else if (TargetRosEnv == ROSTargetEnvironment.APOLLO) { Bridge.Subscribe <Ros.control_command>(APOLLO_CMD_TOPIC, (System.Action <Ros.control_command>)(msg => { lastAutoUpdate = Time.time; var pedals = GetComponent <PedalInputController>(); throttle = pedals.throttleInputCurve.Evaluate((float)msg.throttle / 100); brake = pedals.brakeInputCurve.Evaluate((float)msg.brake / 100); var linearAccel = throttle - brake; var timeStamp = (float)msg.header.timestamp_sec; var steeringTarget = -((float)msg.steering_target) / 100; var dt = timeStamp - lastTimeStamp; lastTimeStamp = timeStamp; var steeringAngle = controller.steerInput; var sgn = Mathf.Sign(steeringTarget - steeringAngle); var steeringRate = (float)msg.steering_rate * sgn; steeringAngle += steeringRate * dt; // to prevent oversteering if (sgn != steeringTarget - steeringAngle) { steeringAngle = steeringTarget; } if (!underKeyboardControl) { autoSteerAngle = steeringAngle; autoInputAccel = linearAccel; } })); } else if (TargetRosEnv == ROSTargetEnvironment.LGSVL) { Bridge.Subscribe <Ros.TwistStamped>(LANEFOLLOWING_CMD_TOPIC, (System.Action <Ros.TwistStamped>)(msg => { lastAutoUpdate = Time.time; autoSteerAngle = (float)msg.twist.angular.x; })); seq = 0; Bridge.AddPublisher <Ros.TwistStamped>(SIMULATOR_CMD_TOPIC); } }
public void OnRosConnected() { Bridge.Subscribe(WHEEL_CMD_TOPIC, (WheelsCmdStampedMsg msg) => { wheelLeftVel = msg.vel_left; wheelRightVel = msg.vel_right; }); // Autoware vehicle command Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (Ros.VehicleCmd msg) => { float WHEEL_SEPARATION = 0.1044197f; //float WHEEL_DIAMETER = 0.065f; float L_GAIN = 0.25f; float A_GAIN = 8.0f; // Assuming that we only get linear in x and angular in z double v = msg.twist_cmd.twist.linear.x; double w = msg.twist_cmd.twist.angular.z; wheelLeftVel = (float)(L_GAIN * v - A_GAIN * w * 0.5 * WHEEL_SEPARATION); wheelRightVel = (float)(L_GAIN * v + A_GAIN * w * 0.5 * WHEEL_SEPARATION); }); string override_topic; if (Bridge.Version == 1) { Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1); override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1; } else { override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2; } Bridge.AddPublisher <BoolStamped>(override_topic); Bridge.Subscribe <BoolStamped>(override_topic, stamped => { ManualControl = stamped.data; }); if (FirstConnection) { var stamp = new BoolStamped() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = seq++, frame_id = "joystick", }, data = true, }; var topic = (Bridge.Version == 1) ? JOYSTICK_OVERRIDE_TOPIC_ROS1 : JOYSTICK_OVERRIDE_TOPIC_ROS2; Bridge.Publish(topic, stamp); FirstConnection = false; } }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Detection2DArray>(objects2DTopicName); } if (targetEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.Subscribe <Ros.DetectedObjectArray>(autowareCameraDetectionTopicName, msg => { if (!isCameraPredictionEnabled || cameraPredictedObjects == null) { return; } foreach (Ros.DetectedObject obj in msg.objects) { var label = obj.label; if (label == "person") { label = "pedestrian"; // Autoware label as person } Ros.Detection2D obj_converted = new Ros.Detection2D() { header = new Ros.Header() { stamp = new Ros.Time() { secs = obj.header.stamp.secs, nsecs = obj.header.stamp.nsecs, }, seq = obj.header.seq, frame_id = obj.header.frame_id, }, id = obj.id, label = label, score = obj.score, bbox = new Ros.BoundingBox2D() { x = obj.x + obj.width / 2, // Autoware (x, y) point at top-left corner y = obj.y + obj.height / 2, width = obj.width, height = obj.height, }, velocity = new Ros.Twist() { linear = new Ros.Vector3() { x = obj.velocity.linear.x, y = 0, z = 0, }, angular = new Ros.Vector3() { x = 0, y = 0, z = obj.velocity.angular.z, }, }, }; cameraPredictedObjects.Add(obj_converted); } cameraPredictedVisuals = cameraPredictedObjects.ToList(); cameraPredictedObjects.Clear(); }); } }
public void OnRosConnected() { Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(CENTER_GRIPPER_SRV, msg => { hook.CenterHook(); return(new Ros.Srv.Empty()); }); Bridge.AddService <Ros.Srv.SetBool, Ros.Srv.SetBoolResponse>(ATTACH_GRIPPER_SRV, msg => { hook.EngageHook(msg.data); return(new Ros.Srv.SetBoolResponse() { success = true, message = "" }); }); // tugbot Bridge.Subscribe(CMD_VEL_TOPIC, (Ros.Twist msg) => { float WHEEL_SEPARATION = 0.515f; float WHEEL_DIAMETER = 0.39273163f; float SCALING_RATIO = 0.208f; // Assuming that we only get linear in x and angular in z double v = msg.linear.x; double w = msg.angular.z; wheelLeftVel = SCALING_RATIO * (float)(v - w * 0.5 * WHEEL_SEPARATION); wheelRightVel = SCALING_RATIO * (float)(v + w * 0.5 * WHEEL_SEPARATION); }); Bridge.Subscribe(WHEEL_CMD_TOPIC, (WheelsCmdStampedMsg msg) => { wheelLeftVel = msg.vel_left; wheelRightVel = msg.vel_right; }); // Autoware vehicle command Bridge.Subscribe(AUTOWARE_CMD_TOPIC, (Ros.VehicleCmd msg) => { float WHEEL_SEPARATION = 0.1044197f; //float WHEEL_DIAMETER = 0.065f; float L_GAIN = 0.25f; float A_GAIN = 8.0f; // Assuming that we only get linear in x and angular in z double v = msg.twist_cmd.twist.linear.x; double w = msg.twist_cmd.twist.angular.z; wheelLeftVel = (float)(L_GAIN * v - A_GAIN * w * 0.5 * WHEEL_SEPARATION); wheelRightVel = (float)(L_GAIN * v + A_GAIN * w * 0.5 * WHEEL_SEPARATION); }); string override_topic; if (Bridge.Version == 1) { Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1); override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1; } else { override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2; } Bridge.AddPublisher <BoolStamped>(override_topic); Bridge.Subscribe <BoolStamped>(override_topic, stamped => { ManualControl = stamped.data; }); if (FirstConnection) { var stamp = new BoolStamped() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = seq++, frame_id = "joystick", }, data = true, }; var topic = (Bridge.Version == 1) ? JOYSTICK_OVERRIDE_TOPIC_ROS1 : JOYSTICK_OVERRIDE_TOPIC_ROS2; Bridge.Publish(topic, stamp); FirstConnection = false; } ManualControl = true; }