public void OnRosConnected() { ImageIsBeingSent = false; #if USE_COMPRESSED Bridge.AddPublisher <Ros.CompressedImage>(TopicName); #else Bridge.AddPublisher <Ros.Image>(TopicName); #endif }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.AddPublisher <Ros.PointCloud2>(topicName); } if (targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.PointCloud2>(ApolloTopicName); } }
public void OnRosConnected() { if (TargetRosEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Apollo.Imu>(ImuTopic); Bridge.AddPublisher <Ros.CorrectedImu>(ApolloIMUOdometryTopic); } else if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1) { Bridge.AddPublisher <Ros.Imu>(ImuTopic); Bridge.AddPublisher <Ros.Odometry>(OdometryTopic); } }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE) { Bridge.AddPublisher <Ros.Sentence>(AutowareTopic); } if (targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.GnssBestPose>(ApolloTopic); } seq = 0; }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE) { Debug.Log("CAN bus not implemented in Autoware (yet). Nothing to publish."); } if (targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Apollo.ChassisMsg>(ApolloTopic); Bridge.AddPublisher <Ros.TwistStamped>(SimulatorTopic); } seq = 0; }
public void OnRosConnected() { if (TargetEnvironment == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.PointCloud2>(ApolloTopicName); } else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE) { Bridge.AddPublisher <Ros.PointCloud2>(AutowareTopicName); } else { Bridge.AddPublisher <Ros.PointCloud2>(TopicName); } }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Detection3DArray>(objects3DTopicName); } }
public void OnRosConnected() { if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1) { Bridge.AddPublisher <Ros.LaserScan>(topicName); } }
public void OnRosConnected() { if (TargetRosEnv == ROSTargetEnvironment.APOLLO) { Bridge.AddPublisher <Ros.Apollo.Imu>(ImuTopic); Bridge.AddPublisher <Ros.CorrectedImu>(ApolloIMUOdometryTopic); } else if (TargetRosEnv == ROSTargetEnvironment.AUTOWARE || TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1) { Bridge.AddPublisher <Ros.Imu>(ImuTopic); Bridge.AddPublisher <Ros.Odometry>(OdometryTopic); Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(ResetOdometry, msg => { odomPosition = new Vector3(0f, 0f, 0f); return(new Ros.Srv.Empty()); }); } }
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.AddPublisher <Ros.Apollo.Drivers.ContiRadar>(ApolloTopicName); }
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 (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.AddPublisher <float>(UNITY_TIME_TOPIC); Bridge.AddPublisher <Ros.Pose>(CAR_INFO_TOPIC); Bridge.AddPublisher <Ros.TwistStamped>(SIM_CUR_VELOCITY_TOPIC); }
public void OnRosConnected() { Bridge.AddPublisher <Ros.Imu>(ImuTopic); seq = 0; }
public void OnRosConnected() { Bridge.AddPublisher <Ros.Imu>(ImuTopic); Bridge.AddPublisher <Ros.CorrectedImu>(ApolloIMUOdometryTopic); }
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 OnRosBridgeAvailable(Ros.Bridge bridge) { Bridge = bridge; Bridge.AddPublisher(this); }
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; }