jmscslgroup / catvehicle Goto Github PK
View Code? Open in Web Editor NEWA macroscopic multivehicle tesbed and hardware-in-the-loop simulator for autonomous driving
Home Page: https://jmscslgroup.github.io/catvehicle
A macroscopic multivehicle tesbed and hardware-in-the-loop simulator for autonomous driving
Home Page: https://jmscslgroup.github.io/catvehicle
Hello,
System
basically in all launch files that spawn the car the argument updateRate
is missing, further the old xacro.py is used (which is not available in noetic) instead of the new xacro.
E.g. see: catvehicle_canyonview.launch or catvehicle_custom.launch
I followed the the tutorial of "Visualizing the CAT Vehicle and Velodyne with rviz", but I was not able to see any point cloud data. I could only see the vehicle itself. Do you have any clues?
Hello I am trying to implement SLAM Gmapping but unfortunately the /map topic in rviz is not recieving any messages. Can you please help?
Hello,
I just noticed that the (ns)/joint_states are out of sequence when I launch roslaunch catvehicle catvehicle_canyonview.launch
.
Steps to reproduce:
roslaunch catvehicle catvehicle_canyonview.launch
(also the updateRate arg is missing in the launch file and as argument for the xacro)rostopic echo -p /catvehicle/joint_states > test.csv
rosrun catvehicle cmdvel2gazebo.py
rosrun teleop_twist_keyboard teleop_twist_keyboard.py cmd_vel:=/catvehicle/cmd_vel
%time | field.header.seq | field.header.stamp | field.header.frame_id | field.name0 | field.name1 | field.name2 | field.name3 | field.name4 | field.name5 | field.position0 | field.position1 | field.position2 | field.position3 | field.position4 | field.position5 | field.velocity0 | field.velocity1 | field.velocity2 | field.velocity3 | field.velocity4 | field.velocity5 |
---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
31950000000 | 3174 | 31950000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.5704995675182 | 21.5692680845664 | -8.76614310030277E-08 | -5.55246391087394E-08 | 20.1174797988895 | 20.1159981330681 | 1.30509378832069 | 1.30509390971527 | 8.17729287286235E-07 | -3.96407053032476E-07 | 1.30509413934305 | 1.305093973141 | |
31953000000 | 3175 | 31953000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.5731097558532 | 21.5718782729832 | 8.88624498429635E-08 | 5.49289964624222E-08 | 1.30509378578268 | 1.30509390758228 | 8.14932136953285E-07 | -3.94486471084148E-07 | 0 | 0 | 0 | 0 | |||
31970000000 | 3176 | 31970000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.5966014506412 | 21.5953699685443 | -8.75578001213739E-08 | -5.55550574432573E-08 | 20.1435816901526 | 20.1421000230534 | 1.30509376292803 | 1.30509388848421 | 7.89617645605951E-07 | -3.77030241746959E-07 | 1.30509411175151 | 1.30509394818482 | |
31973000000 | 3177 | 31973000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.5992116389258 | 21.597980156919 | 8.87183402298319E-08 | 5.49882921418998E-08 | 1.30509376038842 | 1.30509388637455 | 7.86789114795812E-07 | -3.75071877792505E-07 | 0 | 0 | 0 | 0 | |||
31990000000 | 3178 | 31990000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.6227033332597 | 21.6214718521032 | -8.74805845541005E-08 | -5.55860149020759E-08 | 20.1696835808658 | 20.1682019125387 | 1.30509373753871 | 1.305093867497 | 7.61186250763433E-07 | -3.57281033948612E-07 | 1.30509408418741 | 1.30509392294579 | |
31993000000 | 3179 | 31993000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.6253135214939 | 21.6240820404363 | 8.86001316757756E-08 | 5.5048714919792E-08 | 1.30509373500281 | 1.30509386541366 | 7.58325020623361E-07 | -3.55286113047025E-07 | 0 | 0 | 0 | 0 | |||
32000000000 | 320 | 32000000000 | back_right_wheel_joint | back_left_wheel_joint | front_right_steering_joint | front_right_wheel_joint | front_left_steering_joint | front_left_wheel_joint | 0 | 0 | 0 | 0 | 0 | 0 | |||||||
32011000000 | 3180 | 32011000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.6488052153744 | 21.6475737352484 | -8.74294547870136E-08 | -5.56177655042234E-08 | 20.1957854710299 | 20.1943038015188 | 1.3050937122159 | 1.30509384679107 | 7.32422137827116E-07 | -3.37172203938952E-07 | 1.30509405670067 | 1.30509389747672 | |
32013000000 | 3181 | 32013000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.6514154035583 | 21.6501839235404 | 8.85074742384973E-08 | 5.51105054924506E-08 | 1.30509370968924 | 1.30509384473652 | 7.29526938014983E-07 | -3.35142066575305E-07 | 0 | 0 | 0 | 0 | |||
32030000000 | 3182 | 32030000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.6749070969872 | 21.6736756179858 | -8.74040635423512E-08 | -5.5650581032296E-08 | 20.2218873606469 | 20.2204056899895 | 1.3050936870216 | 1.3050938264035 | 7.03312820301147E-07 | -3.16717638359033E-07 | 1.30509402933824 | 1.30509387183286 | |
32033000000 | 3183 | 32033000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.677517285121 | 21.6762858062373 | 8.84399939948821E-08 | 5.51739116616545E-08 | 1.30509368451107 | 1.30509382438325 | 7.00382441453161E-07 | -3.14653732572959E-07 | 0 | 0 | 0 | 0 | |||
32051000000 | 3184 | 32051000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.7010089781014 | 21.6997775003221 | -8.74040209097871E-08 | -5.56847474797451E-08 | 20.2479892497196 | 20.2465075779477 | 1.30509366202036 | 1.30509380637187 | 6.73846497558609E-07 | -2.95932277673651E-07 | 1.30509400215147 | 1.3050938460748 | |
32053000000 | 3185 | 32053000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.7036191661856 | 21.7023876885339 | 8.83972939291766E-08 | 5.52392016572867E-08 | 1.30509365953275 | 1.30509380438894 | 6.70879797215283E-07 | -2.93836152386219E-07 | 0 | 0 | 0 | 0 | |||
32071000000 | 3186 | 32071000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.7271108587214 | 21.7258793822647 | -8.74289360908165E-08 | -5.57205659390547E-08 | 20.2740911382518 | 20.2726094653916 | 1.30509363727564 | 1.30509378672932 | 6.44012154271358E-07 | -2.74832034684823E-07 | 1.3050939751824 | 1.30509382025418 | |
32073000000 | 3187 | 32073000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.7297210467565 | 21.7284895704377 | 8.83789601502372E-08 | 5.53066552555492E-08 | 1.30509363481825 | 1.30509378478792 | 6.41008081555788E-07 | -2.72705331524897E-07 | 0 | 0 | 0 | 0 | |||
32091000000 | 3188 | 32091000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.7532127388531 | 21.7519812638219 | -8.7478399635188E-08 | -5.57583526017424E-08 | 20.3001930262481 | 20.2987113523203 | 1.30509361285705 | 1.3050937675129 | 6.13799796458961E-07 | -2.53433739619951E-07 | 1.30509394848228 | 1.30509379443516 | |
32093000000 | 3189 | 32093000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.7558229268397 | 21.7545914519567 | 8.8384569885136E-08 | 5.53765646671423E-08 | 1.30509361043613 | 1.30509376561658 | 6.10757420233957E-07 | -2.51278190981569E-07 | 0 | 0 | 0 | 0 | |||
32100000000 | 321 | 32100000000 | back_right_wheel_joint | back_left_wheel_joint | front_right_steering_joint | front_right_wheel_joint | front_left_steering_joint | front_left_wheel_joint | 0 | 0 | 0 | 0 | 0 | 0 | |||||||
32110000000 | 3190 | 32110000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.7793146185037 | 21.7780831450022 | -8.75519914345091E-08 | -5.57984360938235E-08 | 20.3262949137142 | 20.3248132387345 | 1.30509358883182 | 1.30509374875692 | 5.83200583331175E-07 | -2.31755056836036E-07 | 1.30509392209665 | 1.30509376867917 | |
32113000000 | 3191 | 32113000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | 21.7819248064426 | 21.7806933330999 | 8.84136923673395E-08 | 5.54492354254421E-08 | 1.30509358645321 | 1.30509374690807 | 5.80119084309298E-07 | -2.2957247045024E-07 | 0 | 0 | 0 | 0 | |||
32131000000 | 3192 | 32131000000 | back_left_wheel_joint | back_right_wheel_joint | front_left_steering_joint | front_right_steering_joint | front_right_wheel_joint | front_left_wheel_joint | 21.8054164976816 | 21.8041850258152 | -8.76492878276736E-08 | -5.58411645812384E-08 | 20.3523968006567 | 20.3509151246358 | 1.30509356526847 | 1.30509373049459 | 5.52206945799202E-07 | -2.09814387575323E-07 | 1.30509389607071 | 1.30509374304483 |
Expected Results
Same behaviour as with eg turtlebot3:
roslaunch turtlebot3_gazebo turtlebot3_world.launch
roslaunch turtlebot3_bringup turtlebot3_remote.launch
rostopic echo -p /joint_states > turtl.csv
rosrun turtlebot3_teleop turtlebot3_teleop_key
%time | field.header.seq | field.header.stamp | field.header.frame_id | field.name0 | field.name1 | field.position0 | field.position1 |
---|---|---|---|---|---|---|---|
88665000000 | 2655 | 88665000000 | wheel_right_joint | wheel_left_joint | 0.0018757223517340549 | -0.0019258201689602572 | |
88698000000 | 2656 | 88698000000 | wheel_right_joint | wheel_left_joint | 0.0018764244494224869 | -0.0019265490380302452 | |
88730000000 | 2657 | 88730000000 | wheel_right_joint | wheel_left_joint | 0.001877126548953001 | -0.0019272779090311332 | |
88765000000 | 2658 | 88765000000 | wheel_right_joint | wheel_left_joint | 0.0018778499261413728 | -0.001928028869048859 | |
88797000000 | 2659 | 88797000000 | wheel_right_joint | wheel_left_joint | 0.00187855202938092 | -0.0019287577439701664 | |
88830000000 | 2660 | 88830000000 | wheel_right_joint | wheel_left_joint | 0.0018792541344501146 | -0.001929486620818821 | |
88865000000 | 2661 | 88865000000 | wheel_right_joint | wheel_left_joint | 0.001879977517345921 | -0.00193023758687616 | |
88898000000 | 2662 | 88898000000 | wheel_right_joint | wheel_left_joint | 0.0018806796261241487 | -0.0019309664676772087 | |
88930000000 | 2663 | 88930000000 | wheel_right_joint | wheel_left_joint | 0.0018813817367231422 | -0.0019316953504269208 | |
88964000000 | 2664 | 88964000000 | wheel_right_joint | wheel_left_joint | 0.0018821051253361532 | -0.0019324463225327548 |
Partly workaround:
catvehicle.launch
(used by all launch files afaik), lines 60 - 62 This will however still sometimes publish more and less joint_states. I guess there is a problem with the controller_manager.
Hello again,
in the comment in the xacro file you state that the wheelbase is 2.8232111 m, in the file that computes the velocities for the driven wheels you state that it is 2.62 m which is also used in the distanceEstimatorSteeringbased.cpp and when I run rosservice call /gazebo/get_link_state "link_name: 'catvehicle::back_left_wheel_link' reference_frame: 'catvehicle::front_left_wheel_link'"
(on a paused simulation, before the joints are moved by the car bouncing around) i get 2.6 m
For the wheel trad I receive a different value for the front and rear wheels using the gazebo service:
rosservice call /gazebo/get_link_state "link_name: 'catvehicle::back_right_wheel_link' reference_frame: 'catvehicle::back_left_wheel_link'"
-> 1.53 m
rosservice call /gazebo/get_link_state "link_name: 'catvehicle::front_right_wheel_link' reference_frame: 'catvehicle::front_left_wheel_link'"
-> 1.49 m
But in cmdvel2gazebo it is set to 1.29 m
Why do you calculate the simplified steering angle (I assume you are using a bicycle model) using (see):
a0 = steering_joints[0]->Position(0);
//a1 = steering_joints[1]->GetAngle(0).Radian();
a1 = steering_joints[1]->Position(0);
// average these values, though in most modes they will be equal
steering_msg.torque.z = (a0+a1)/2.0;
In Jazar, R. N. (2017). Vehicle dynamics: theory and application. Springer. equation 7.4 the steering angle for the simplified bicycle is computed as:
So imho it should be smth like
index 9fe0ea1..c7762bc 100644
--- a/src/cont.cc
+++ b/src/cont.cc
@@ -46,6 +46,7 @@ TO PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
#include "std_msgs/String.h"
#include "catvehicle/cont.hh"
#include <tf/transform_broadcaster.h>
+#include <math.h>
using namespace std;
@@ -200,7 +201,13 @@ namespace gazebo
a1 = steering_joints[1]->Position(0);
// average these values, though in most modes they will be equal
- steering_msg.torque.z = (a0+a1)/2.0;
+ // steering_msg.torque.z = (a0+a1)/2.0;
+
+ double cot_left = 1/tan(a0);
+ double cot_right = 1/tan(a1);
+ double cot_steer = (cot_left + cot_right)/2;
+ double steering_angle = atan(1/cot_steer);
+ steering_msg.torque.z = steering_angle;
steering_pub.publish(steering_msg);
Hi everyone. I watched Jonathan Sprinkle's rviz visualize video. When I want to add RobotModel, it just adds sensors, not model.
When I look at the command line while skidpan launch file loading, I get this error.
[INFO] [1616678881.408235, 1089.346000]: Spawn status: SpawnModel: Entity pushed to spawn queue, but spawn service timed out waiting for entity to appear in simulation under the name catvehicle
[ERROR] [1616678881.409398, 1089.346000]: Spawn service failed. Exiting.
[ INFO] [1616678881.423752592, 1089.346000000]: Physics dynamic reconfigure ready.
[catvehicle/urdf_spawnercatvehicle-2] process has died [pid 12118, exit code 1, cmd /opt/ros/melodic/lib/gazebo_ros/spawn_model -x 1 -y 1 -z 0 -urdf -model catvehicle -param robot_description __name:=urdf_spawnercatvehicle __log:=/home/kadir/.ros/log/fa1909d0-8d51-11eb-aa58-e82a44f292f1/catvehicle-urdf_spawnercatvehicle-2.log].
log file: /home/kadir/.ros/log/fa1909d0-8d51-11eb-aa58-e82a44f292f1/catvehicle-urdf_spawnercatvehicle-2*.log
[ INFO] [1616678881.789254929, 1089.346000000]: Camera Plugin: Using the 'robotNamespace' param: '/catvehicle'
I did launch catvehicle_skidpan.launch without arguments. So I did not change anything.
I am using Ubuntu 18.04 ROS Melodic
I hope I give to enough information.
I am using ros melodic. Ubuntu 18.04
Car moves backward successfully but it can not go forward. Please help me
A declarative, efficient, and flexible JavaScript library for building user interfaces.
๐ Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
An Open Source Machine Learning Framework for Everyone
The Web framework for perfectionists with deadlines.
A PHP framework for web artisans
Bring data to life with SVG, Canvas and HTML. ๐๐๐
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
Some thing interesting about web. New door for the world.
A server is a program made to process requests and deliver data to clients.
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
Some thing interesting about visualization, use data art
Some thing interesting about game, make everyone happy.
We are working to build community through open source technology. NB: members must have two-factor auth.
Open source projects and samples from Microsoft.
Google โค๏ธ Open Source for everyone.
Alibaba Open Source for everyone
Data-Driven Documents codes.
China tencent open source team.