frame_id of scan is changed

Author: esm-tmori

Assets/Scripts/Hakoniwa/PluggableAsset/Assets/Robot/TB3/LaserScanner.cs

@@ -34,7 +34,7 @@ public void Initialize(object root)
         public void UpdateSensorData(Pdu pdu)
         {
             TimeStamp.Set(pdu);
-            pdu.Ref("header").SetData("frame_id", "laser");
+            pdu.Ref("header").SetData("frame_id", "base_scan");
 
             pdu.SetData("angle_min", angle_min);
             pdu.SetData("angle_max", angle_max);

Assets/Scripts/Hakoniwa/PluggableAsset/Assets/Robot/TB3/Motor.cs

@@ -7,7 +7,7 @@ namespace Hakoniwa.PluggableAsset.Assets.Robot.TB3
 {
     public class Motor : MonoBehaviour
     {
-        private float power_const = 100;
+        private float power_const = 1000;
         private int motor_velocity_av_count = 0;
         private int motor_velocity_av_max = 10;
         private float motor_sum_degree = 0.0f;

Assets/Scripts/Hakoniwa/PluggableAsset/Assets/Robot/TB3/MotorController.cs

@@ -12,7 +12,7 @@ public class MotorController
         private float[] prev_angle = new float[2];  // 0: R, 1: L
         private float[] delta_angle = new float[2];  // 0: R, 1: L
         private float[] moving_distance = new float[2];  // 0: R, 1: L
-        private int motor_power = 10;
+        private int motor_power = 500;
         private float motor_interval_distance = 16.0f; // 16cm
         private IPduReader pdu_reader;
 

Assets/Scripts/Hakoniwa/PluggableAsset/Assets/Robot/TB3/RobotController.cs

@@ -200,6 +200,7 @@ private void CalcOdometry()
         public void DoActuation()
         {
             this.motor_controller.DoActuation();
+            //Physics.SyncTransforms();
         }
 
         public string GetName()

Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Method/ROS/EV3_TB3/RosTopicIo.cs

@@ -79,6 +79,7 @@ public RosTopicIo()
 			ros.RegisterPublisher<ImuMsg>("imu");
 			ros.RegisterPublisher<OdometryMsg>("odom");
 			ros.RegisterPublisher<TFMessageMsg>("tf");
+			ros.RegisterPublisher<JointStateMsg>("joint_states");
             ros.Subscribe<TwistMsg>("cmd_vel", TwistMsgChange);
 
         }

Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/ROS/EV3_TB3/RosTopicPduReaderConverter.cs

@@ -118,6 +118,14 @@ private void ConvertToPdu(ImuMsg src, IPduWriteOperation dst)
 			ConvertToPdu(src.linear_acceleration, dst.Ref("linear_acceleration").GetPduWriteOps());
             dst.SetData("linear_acceleration_covariance", src.linear_acceleration_covariance);
         }
+        private void ConvertToPdu(JointStateMsg src, IPduWriteOperation dst)
+        {
+			ConvertToPdu(src.header, dst.Ref("header").GetPduWriteOps());
+            dst.SetData("name", src.name);
+            dst.SetData("position", src.position);
+            dst.SetData("velocity", src.velocity);
+            dst.SetData("effort", src.effort);
+        }
         private void ConvertToPdu(LaserScanMsg src, IPduWriteOperation dst)
         {
 			ConvertToPdu(src.header, dst.Ref("header").GetPduWriteOps());
@@ -247,6 +255,12 @@ public void ConvertToPduData(IPduCommData src, IPduReader dst)
                 ConvertToPdu(ros_topic_data, dst.GetWriteOps());
                 return;
             }
+            if (ros_pdu_reader.GetTypeName().Equals("sensor_msgs/JointState"))
+            {
+                var ros_topic_data = ros_topic.GetTopicData() as JointStateMsg;
+                ConvertToPdu(ros_topic_data, dst.GetWriteOps());
+                return;
+            }
             if (ros_pdu_reader.GetTypeName().Equals("geometry_msgs/Twist"))
             {
                 var ros_topic_data = ros_topic.GetTopicData() as TwistMsg;

Assets/Scripts/Hakoniwa/PluggableAsset/Communication/Pdu/ROS/EV3_TB3/RosTopicPduWriterConverter.cs

@@ -130,6 +130,14 @@ static private void ConvertToMessage(IPduReadOperation src, ImuMsg dst)
             ConvertToMessage(src.Ref("linear_acceleration").GetPduReadOps(), dst.linear_acceleration);
 			dst.linear_acceleration_covariance = src.GetDataFloat64Array("linear_acceleration_covariance");
         }
+        static private void ConvertToMessage(IPduReadOperation src, JointStateMsg dst)
+        {
+            ConvertToMessage(src.Ref("header").GetPduReadOps(), dst.header);
+			dst.name = src.GetDataStringArray("name");
+			dst.position = src.GetDataFloat64Array("position");
+			dst.velocity = src.GetDataFloat64Array("velocity");
+			dst.effort = src.GetDataFloat64Array("effort");
+        }
         static private void ConvertToMessage(IPduReadOperation src, LaserScanMsg dst)
         {
             ConvertToMessage(src.Ref("header").GetPduReadOps(), dst.header);
@@ -261,6 +269,12 @@ static public Message ConvertToMessage(IPduReadOperation src, string type)
                 ConvertToMessage(src, ros_topic);
                 return ros_topic;
             }
+            if (type.Equals("sensor_msgs/JointState"))
+            {
+            	JointStateMsg ros_topic = new JointStateMsg();
+                ConvertToMessage(src, ros_topic);
+                return ros_topic;
+            }
             if (type.Equals("geometry_msgs/Twist"))
             {
             	TwistMsg ros_topic = new TwistMsg();