From: Walter Fetter Lages Date: Thu, 9 Dec 2021 05:09:25 +0000 (-0300) Subject: Raul TCC final version. X-Git-Tag: raulTCC^0 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=fcf5e3ac0dac03f771fc8e92f1d696b2db844c1e;p=twil.git Raul TCC final version. --- diff --git a/twil_description/xacro/twil.urdf.xacro b/twil_description/xacro/twil.urdf.xacro index b75a0fd..73c3dfc 100644 --- a/twil_description/xacro/twil.urdf.xacro +++ b/twil_description/xacro/twil.urdf.xacro @@ -159,12 +159,12 @@ - 15 + 90 1.309 - 640 - 480 + 1280 + 720 R8G8B8 @@ -191,17 +191,20 @@ /camera/depth/camera_info /camera/depth/points plugin_camera_link - 0.175 + 0.28 10.0 0 0 0 0 0 - 0 + + 0 + 0 + 0 0 0 - 0 + 0 0 diff --git a/twil_location/launch/gazebo.launch b/twil_location/launch/gazebo.launch index df9ad20..44636bf 100644 --- a/twil_location/launch/gazebo.launch +++ b/twil_location/launch/gazebo.launch @@ -3,16 +3,13 @@ - + - + + - - - - @@ -29,34 +26,15 @@ - - - - - - - - - - + - - - - - - - + - - - - diff --git a/twil_location/launch/location.launch b/twil_location/launch/location.launch index db5a56c..3d27cdd 100644 --- a/twil_location/launch/location.launch +++ b/twil_location/launch/location.launch @@ -1,112 +1,161 @@ - + - - + + - + - + + + + - + - - + + - + - + - + - + - + - - - - - - - - - - - + + + + + + + + + + + - - - - - + + + + + - - - + + + + + + + + - + + - + + - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/twil_location/launch/new_world.launch b/twil_location/launch/new_world.launch deleted file mode 100644 index b6a4011..0000000 --- a/twil_location/launch/new_world.launch +++ /dev/null @@ -1,18 +0,0 @@ - - - - - - - - - - - - - - - - - - diff --git a/twil_location/launch/plot.launch b/twil_location/launch/plot.launch deleted file mode 100644 index 9525948..0000000 --- a/twil_location/launch/plot.launch +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/twil_location/rviz/twil_mapping.rviz b/twil_location/rviz/twil_mapping.rviz index 35432ac..892ea31 100644 --- a/twil_location/rviz/twil_mapping.rviz +++ b/twil_location/rviz/twil_mapping.rviz @@ -5,8 +5,9 @@ Panels: Property Tree Widget: Expanded: - /DepthCloud1/Auto Size1 + - /Odometry2/Shape1 Splitter Ratio: 0.5 - Tree Height: 370 + Tree Height: 299 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -25,7 +26,7 @@ Panels: Experimental: true Name: Time SyncMode: 0 - SyncSource: DepthCloud + SyncSource: "" Preferences: PromptSaveOnExit: true Toolbars: @@ -253,44 +254,7 @@ Visualization Manager: Topic: /twist_mrac_linearizing_controller/odom Unreliable: false Value: true - - Alpha: 0.699999988079071 - - Class: rviz/Odometry - Covariance: - Orientation: - Alpha: 0.5 - Color: 255; 255; 127 - Color Style: Unique - Frame: Local - Offset: 1 - Scale: 1 - Value: true - Position: - Alpha: 0.30000001192092896 - Color: 204; 51; 204 - Scale: 1 - Value: true - Value: true - Enabled: true - Keep: 42 - Name: rgbd_Odometry - Position Tolerance: 0.30000001192092896 - Queue Size: 10 - Shape: - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Color: 25; 255; 0 - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Value: Arrow - Topic: /rgbd_odom - Unreliable: false - Value: true - - Alpha: 0.699999988079071 - + - Angle Tolerance: 0.10000000149011612 Class: rviz/Odometry Covariance: Orientation: @@ -326,10 +290,6 @@ Visualization Manager: Unreliable: false Value: true - Alpha: 0.699999988079071 - - - - Class: rviz/Map Color Scheme: costmap Draw Behind: false @@ -341,26 +301,12 @@ Visualization Manager: Value: true - Alpha: 1 Axes Length: 1 - Axes Radius: 0.10000000149011612 - - Class: rviz/Map - Color Scheme: map - Draw Behind: false - Enabled: true - Name: Map(SLAM) - Topic: /Slam_map - Unreliable: false - Use Timestamp: false - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - + Axes Radius: 0.10000000149011612 Class: rviz/Pose Color: 255; 255; 0 Enabled: true Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 + Head Radius: 0.10000000149011612 Name: Pose (goal) Queue Size: 10 Shaft Length: 1 @@ -415,9 +361,7 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 4096 Min Color: 0; 0; 0 - Min Intensity: 0 Name: DepthCloud Occlusion Compensation: Occlusion Time-Out: 30 @@ -449,8 +393,10 @@ Visualization Manager: Odometry: true Path: true Pose (goal): true + PoseWithCovariance: true RobotModel: true Value: true + ground_truth: true Zoom Factor: 1 - Class: rviz/Camera Enabled: true @@ -470,9 +416,108 @@ Visualization Manager: Odometry: true Path: true Pose (goal): true + PoseWithCovariance: true RobotModel: true Value: true + ground_truth: true Zoom Factor: 1 + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 25; 255; 25 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /amcl_pose + Unreliable: false + Value: true + - Angle Tolerance: 0.10000000149011612 + Class: rviz/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Keep: 100 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Queue Size: 10 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 255; 25 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: /rgbd_odom + Unreliable: false + Value: true + - Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz/PoseWithCovariance + Color: 255; 25; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: PoseWithCovariance + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Arrow + Topic: /robot_pose_ekf/odom_combined + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -528,10 +573,10 @@ Window Geometry: collapsed: false Displays: collapsed: false - Height: 1025 + Height: 911 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd00000004000000000000015600000367fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000001fb000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c00430061006d00650072006100200028006400650070007400680029010000023c000000980000001600fffffffb0000001c00430061006d006500720061002000280063006f006c006f0072002901000002da000000c80000001600ffffff000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002c400fffffffb0000000800540069006d00650100000000000004500000000000000000000005e10000036700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000400000000000001fd000002f1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001b6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001c00430061006d0065007200610020002800640065007000740068002901000001f9000000830000001600fffffffb0000001c00430061006d006500720061002000280063006f006c006f007200290100000282000000ac0000001600ffffff000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000031b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000535000002f100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -540,6 +585,6 @@ Window Geometry: collapsed: false Views: collapsed: true - Width: 1853 - X: 59 - Y: 4 + Width: 1848 + X: 72 + Y: 27 diff --git a/twil_location/src/twil_location.cpp b/twil_location/src/twil_location.cpp index 2b809bb..8b9a8fd 100644 --- a/twil_location/src/twil_location.cpp +++ b/twil_location/src/twil_location.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include @@ -15,12 +16,13 @@ class location location(ros::NodeHandle node); ~location(void); void dataFusion(void); - + private: ros::NodeHandle node_; - + ros::Subscriber visualodometrySubscriber; - ros::Subscriber odometrySubscriber; + ros::Subscriber odometrySubscriber; + ros::Subscriber efkPoseSubscriber; ros::Publisher fusedOdomPublisher; const int nJoints; @@ -32,64 +34,78 @@ class location Eigen::MatrixXd P2; ros::Time lastTime; - + std::vector wheelRadius; double wheelBase; - - nav_msgs::Odometry _odometry; + + nav_msgs::Odometry odom; nav_msgs::Odometry _rgbd_odometry; - + geometry_msgs::PoseWithCovarianceStamped _ekf_pose; + void VisualOdomCB(const nav_msgs::Odometry::ConstPtr &rgbd_odometry); - void OdomCB(const nav_msgs::Odometry::ConstPtr &odometry); + void OdomCB(const nav_msgs::Odometry::ConstPtr &wheel_odom); + void ekfCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &ekf_pose); }; location::location(ros::NodeHandle node): - nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),wheelRadius(nJoints),_odometry() + nJoints(2),u(nJoints),thetaEst1(nJoints),thetaEst2(nJoints),P1(nJoints,nJoints),P2(nJoints,nJoints),wheelRadius(nJoints),odom() { node_=node; - + visualodometrySubscriber= node_.subscribe("rgbd_odom",1000,&location::VisualOdomCB,this); - odometrySubscriber = node_.subscribe("odom",1000,&location::OdomCB,this); - - fusedOdomPublisher=node_.advertise("odom_fused",1000); - + odometrySubscriber = node_.subscribe("wheel_odom",1000,&location::OdomCB,this); + efkPoseSubscriber = node_.subscribe("robot_pose_ekf/odom_combined", 1000,&location::ekfCB,this); + + fusedOdomPublisher=node_.advertise("odom",1000); + lastTime=ros::Time::now(); - } location::~location(void) { visualodometrySubscriber.shutdown(); odometrySubscriber.shutdown(); + efkPoseSubscriber.shutdown(); } -void location::OdomCB(const nav_msgs::Odometry::ConstPtr &odometry) +void location::OdomCB(const nav_msgs::Odometry::ConstPtr &wheel_odom) { - _odometry.header.seq = odometry->header.seq; - _odometry.header.stamp = odometry->header.stamp; - _odometry.header.frame_id = odometry->header.frame_id; - - _odometry.pose.pose.position.x = odometry->pose.pose.position.x; - _odometry.pose.pose.position.y = odometry->pose.pose.position.y; - _odometry.pose.pose.position.z = odometry->pose.pose.position.z; - - _odometry.pose.pose.orientation.x = odometry->pose.pose.orientation.x; - _odometry.pose.pose.orientation.y = odometry->pose.pose.orientation.y; - _odometry.pose.pose.orientation.z = odometry->pose.pose.orientation.z; - _odometry.pose.pose.orientation.w = odometry->pose.pose.orientation.w; - - _odometry.pose.covariance = odometry->pose.covariance; - - _odometry.twist.twist.linear.x = odometry->twist.twist.linear.x; - _odometry.twist.twist.linear.y = odometry->twist.twist.linear.y; - _odometry.twist.twist.linear.z = odometry->twist.twist.linear.z; - - _odometry.twist.twist.angular.x = odometry->twist.twist.angular.x; - _odometry.twist.twist.angular.y = odometry->twist.twist.angular.y; - _odometry.twist.twist.angular.z = odometry->twist.twist.angular.z; - - _odometry.twist.covariance = odometry->twist.covariance; + odom.header.seq = wheel_odom->header.seq; + odom.header.stamp = wheel_odom->header.stamp; + odom.header.frame_id = wheel_odom->header.frame_id; + + odom.child_frame_id = wheel_odom->child_frame_id; + + odom.pose.pose.position.x = wheel_odom->pose.pose.position.x; + odom.pose.pose.position.y = wheel_odom->pose.pose.position.y; + odom.pose.pose.position.z = wheel_odom->pose.pose.position.z; + + odom.pose.pose.orientation.x = wheel_odom->pose.pose.orientation.x; + odom.pose.pose.orientation.y = wheel_odom->pose.pose.orientation.y; + odom.pose.pose.orientation.z = wheel_odom->pose.pose.orientation.z; + odom.pose.pose.orientation.w = wheel_odom->pose.pose.orientation.w; + + odom.pose.covariance = wheel_odom->pose.covariance; + + odom.twist.twist.linear.x = wheel_odom->twist.twist.linear.x; + odom.twist.twist.linear.y = wheel_odom->twist.twist.linear.y; + odom.twist.twist.linear.z = wheel_odom->twist.twist.linear.z; + + odom.twist.twist.angular.x = wheel_odom->twist.twist.angular.x; + odom.twist.twist.angular.y = wheel_odom->twist.twist.angular.y; + odom.twist.twist.angular.z = wheel_odom->twist.twist.angular.z; + + odom.twist.covariance = wheel_odom->twist.covariance; +} + +void location::ekfCB(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &ekf_pose) +{ + _ekf_pose.header.seq = ekf_pose->header.seq; + _ekf_pose.header.stamp = ekf_pose->header.stamp; + _ekf_pose.header.frame_id = ekf_pose->header.frame_id; + + _ekf_pose.pose = ekf_pose->pose; } void location::VisualOdomCB(const nav_msgs::Odometry::ConstPtr &rgbd_odometry) @@ -97,32 +113,36 @@ void location::VisualOdomCB(const nav_msgs::Odometry::ConstPtr &rgbd_odometry) _rgbd_odometry.header.seq = rgbd_odometry->header.seq; _rgbd_odometry.header.stamp = rgbd_odometry->header.stamp; _rgbd_odometry.header.frame_id = rgbd_odometry->header.frame_id; - + + _rgbd_odometry.child_frame_id = rgbd_odometry->child_frame_id; + _rgbd_odometry.pose.pose.position.x = rgbd_odometry->pose.pose.position.x; _rgbd_odometry.pose.pose.position.y = rgbd_odometry->pose.pose.position.y; _rgbd_odometry.pose.pose.position.z = rgbd_odometry->pose.pose.position.z; - + _rgbd_odometry.pose.pose.orientation.x = rgbd_odometry->pose.pose.orientation.x; _rgbd_odometry.pose.pose.orientation.y = rgbd_odometry->pose.pose.orientation.y; _rgbd_odometry.pose.pose.orientation.z = rgbd_odometry->pose.pose.orientation.z; _rgbd_odometry.pose.pose.orientation.w = rgbd_odometry->pose.pose.orientation.w; - + _rgbd_odometry.pose.covariance = rgbd_odometry->pose.covariance; - + _rgbd_odometry.twist.twist.linear.x = rgbd_odometry->twist.twist.linear.x; _rgbd_odometry.twist.twist.linear.y = rgbd_odometry->twist.twist.linear.y; _rgbd_odometry.twist.twist.linear.z = rgbd_odometry->twist.twist.linear.z; - + _rgbd_odometry.twist.twist.angular.x = rgbd_odometry->twist.twist.angular.x; _rgbd_odometry.twist.twist.angular.y = rgbd_odometry->twist.twist.angular.y; _rgbd_odometry.twist.twist.angular.z = rgbd_odometry->twist.twist.angular.z; - + _rgbd_odometry.twist.covariance = rgbd_odometry->twist.covariance; } void location::dataFusion() { - fusedOdomPublisher.publish(_odometry); + odom.pose = _ekf_pose.pose; + + fusedOdomPublisher.publish(odom); } @@ -130,14 +150,15 @@ int main(int argc,char* argv[]) { ros::init(argc,argv,"twil_location"); ros::NodeHandle node; - + location location(node); ros::Rate loop(100); while(ros::ok()) { + location.dataFusion(); - + ros::spinOnce(); loop.sleep(); }