diff --git a/darwin_control/config/darwin_cpp.yaml b/darwin_control/config/darwin_cpp.yaml
index 1f61725bb7be5b68d51a721333f0e99d1c26a267..2d1b7e9288c658dc2770dacf79b9005969584275 100644
--- a/darwin_control/config/darwin_cpp.yaml
+++ b/darwin_control/config/darwin_cpp.yaml
@@ -5,8 +5,8 @@ darwin:
publish_rate: 50
darwin_controller_cpp:
- dyn_serial_device: '/dev/pts/19'
- imu_serial_device: '/dev/pts/22'
+ dyn_serial_device: '/dev/pts/11'
+ imu_serial_device: '/dev/pts/25'
type: effort_controllers/DarwinControllerCPP
joints:
- j_shoulder_pitch_r
diff --git a/darwin_description/config/darwin_rviz.rviz b/darwin_description/config/darwin_rviz.rviz
index ff2c2c5984f93e264ece48cec0a5bc7bcdab428a..39d7dc4275669652fb9e57475023a76ff454d021 100644
--- a/darwin_description/config/darwin_rviz.rviz
+++ b/darwin_description/config/darwin_rviz.rviz
@@ -30,6 +30,8 @@ Panels:
Name: Time
SyncMode: 0
SyncSource: ""
+Toolbars:
+ toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
@@ -104,46 +106,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
- left_down_left_front_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_down_left_middle_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_down_left_rear_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_down_right_front_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_down_right_middle_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_down_right_rear_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_front_left_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- left_front_right_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
left_leg_ankle_pitch:
Alpha: 1
Show Axes: false
@@ -194,46 +156,6 @@ Visualization Manager:
Show Axes: false
Show Trail: false
Value: true
- right_down_left_front_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_down_left_middle_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_down_left_rear_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_down_right_front_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_down_right_middle_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_down_right_rear_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_front_left_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
- right_front_right_ir_link:
- Alpha: 1
- Show Axes: false
- Show Trail: false
- Value: true
right_leg_ankle_pitch:
Alpha: 1
Show Axes: false
@@ -280,6 +202,58 @@ Visualization Manager:
Frame Timeout: 15
Frames:
All Enabled: true
+ E125:
+ Value: true
+ E175:
+ Value: true
+ E225:
+ Value: true
+ E25:
+ Value: true
+ E75:
+ Value: true
+ N125:
+ Value: true
+ N125_cam:
+ Value: true
+ N125_det:
+ Value: true
+ N175:
+ Value: true
+ N175_cam:
+ Value: true
+ N175_det:
+ Value: true
+ N25:
+ Value: true
+ N25_cam:
+ Value: true
+ N25_det:
+ Value: true
+ N75:
+ Value: true
+ N75_cam:
+ Value: true
+ N75_det:
+ Value: true
+ S125:
+ Value: true
+ S175:
+ Value: true
+ S25:
+ Value: true
+ S75:
+ Value: true
+ W125:
+ Value: true
+ W175:
+ Value: true
+ W225:
+ Value: true
+ W25:
+ Value: true
+ W75:
+ Value: true
darwin/base_footprint:
Value: true
darwin/base_link:
@@ -300,22 +274,6 @@ Visualization Manager:
Value: true
darwin/left_arm_low:
Value: true
- darwin/left_down_left_front_ir_link:
- Value: true
- darwin/left_down_left_middle_ir_link:
- Value: true
- darwin/left_down_left_rear_ir_link:
- Value: true
- darwin/left_down_right_front_ir_link:
- Value: true
- darwin/left_down_right_middle_ir_link:
- Value: true
- darwin/left_down_right_rear_ir_link:
- Value: true
- darwin/left_front_left_ir_link:
- Value: true
- darwin/left_front_right_ir_link:
- Value: true
darwin/left_leg_ankle_pitch:
Value: true
darwin/left_leg_ankle_roll:
@@ -338,35 +296,63 @@ Visualization Manager:
Value: true
darwin/right_arm_low:
Value: true
- darwin/right_down_left_front_ir_link:
+ darwin/right_leg_ankle_pitch:
Value: true
- darwin/right_down_left_middle_ir_link:
+ darwin/right_leg_ankle_roll:
Value: true
- darwin/right_down_left_rear_ir_link:
+ darwin/right_leg_knee:
Value: true
- darwin/right_down_right_front_ir_link:
+ darwin/right_leg_pelvis_pitch:
Value: true
- darwin/right_down_right_middle_ir_link:
+ darwin/right_leg_pelvis_roll:
Value: true
- darwin/right_down_right_rear_ir_link:
+ darwin/right_leg_pelvis_yaw:
Value: true
- darwin/right_front_left_ir_link:
+ darwin/right_shoulder:
Value: true
- darwin/right_front_right_ir_link:
+ obstacle_base_fence_link:
Value: true
- darwin/right_leg_ankle_pitch:
+ obstacle_base_link:
Value: true
- darwin/right_leg_ankle_roll:
+ obstacles_env_empty/E125:
Value: true
- darwin/right_leg_knee:
+ obstacles_env_empty/E175:
Value: true
- darwin/right_leg_pelvis_pitch:
+ obstacles_env_empty/E225:
Value: true
- darwin/right_leg_pelvis_roll:
+ obstacles_env_empty/E25:
Value: true
- darwin/right_leg_pelvis_yaw:
+ obstacles_env_empty/E75:
Value: true
- darwin/right_shoulder:
+ obstacles_env_empty/N125:
+ Value: true
+ obstacles_env_empty/N175:
+ Value: true
+ obstacles_env_empty/N25:
+ Value: true
+ obstacles_env_empty/N75:
+ Value: true
+ obstacles_env_empty/S125:
+ Value: true
+ obstacles_env_empty/S175:
+ Value: true
+ obstacles_env_empty/S25:
+ Value: true
+ obstacles_env_empty/S75:
+ Value: true
+ obstacles_env_empty/W125:
+ Value: true
+ obstacles_env_empty/W175:
+ Value: true
+ obstacles_env_empty/W225:
+ Value: true
+ obstacles_env_empty/W25:
+ Value: true
+ obstacles_env_empty/W75:
+ Value: true
+ obstacles_env_empty/obstacle_base_fence_link:
+ Value: true
+ obstacles_env_empty/obstacle_base_link:
Value: true
Marker Scale: 0.200000003
Name: TF
@@ -374,72 +360,96 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
- darwin/odom:
- darwin/base_footprint:
- darwin/base_link:
- darwin/darwin_accel:
- {}
- darwin/darwin_gyro:
- {}
- darwin/darwin_imu_link:
- {}
- darwin/left_leg_pelvis_yaw:
- darwin/left_leg_pelvis_roll:
- darwin/left_leg_pelvis_pitch:
- darwin/left_leg_knee:
- darwin/left_leg_ankle_pitch:
- darwin/left_leg_ankle_roll:
- darwin/left_down_left_front_ir_link:
- {}
- darwin/left_down_left_middle_ir_link:
- {}
- darwin/left_down_left_rear_ir_link:
- {}
- darwin/left_down_right_front_ir_link:
- {}
- darwin/left_down_right_middle_ir_link:
- {}
- darwin/left_down_right_rear_ir_link:
+ obstacle_base_link:
+ E125:
+ {}
+ E175:
+ {}
+ E225:
+ {}
+ E25:
+ {}
+ E75:
+ {}
+ N125:
+ {}
+ N175:
+ {}
+ N25:
+ {}
+ N75:
+ {}
+ S125:
+ {}
+ S175:
+ {}
+ S25:
+ {}
+ S75:
+ {}
+ W125:
+ {}
+ W175:
+ {}
+ W225:
+ {}
+ W25:
+ {}
+ W75:
+ {}
+ darwin/odom:
+ darwin/base_footprint:
+ darwin/base_link:
+ darwin/darwin_accel:
+ {}
+ darwin/darwin_gyro:
+ {}
+ darwin/darwin_imu_link:
+ {}
+ darwin/left_leg_pelvis_yaw:
+ darwin/left_leg_pelvis_roll:
+ darwin/left_leg_pelvis_pitch:
+ darwin/left_leg_knee:
+ darwin/left_leg_ankle_pitch:
+ darwin/left_leg_ankle_roll:
{}
- darwin/left_front_left_ir_link:
- {}
- darwin/left_front_right_ir_link:
- {}
- darwin/left_shoulder:
- darwin/left_arm_high:
- darwin/left_arm_low:
- {}
- darwin/neck:
- darwin/head:
- darwin/darwin_cam_link:
- darwin/darwin_cam_optical_link:
+ darwin/left_shoulder:
+ darwin/left_arm_high:
+ darwin/left_arm_low:
{}
- darwin/right_leg_pelvis_yaw:
- darwin/right_leg_pelvis_roll:
- darwin/right_leg_pelvis_pitch:
- darwin/right_leg_knee:
- darwin/right_leg_ankle_pitch:
- darwin/right_leg_ankle_roll:
- darwin/right_down_left_front_ir_link:
- {}
- darwin/right_down_left_middle_ir_link:
- {}
- darwin/right_down_left_rear_ir_link:
- {}
- darwin/right_down_right_front_ir_link:
- {}
- darwin/right_down_right_middle_ir_link:
- {}
- darwin/right_down_right_rear_ir_link:
+ darwin/neck:
+ darwin/head:
+ darwin/darwin_cam_link:
+ darwin/darwin_cam_optical_link:
+ N125_cam:
+ {}
+ N125_det:
+ {}
+ N175_cam:
+ {}
+ N175_det:
+ {}
+ N25_cam:
+ {}
+ N25_det:
+ {}
+ N75_cam:
+ {}
+ N75_det:
+ {}
+ darwin/right_leg_pelvis_yaw:
+ darwin/right_leg_pelvis_roll:
+ darwin/right_leg_pelvis_pitch:
+ darwin/right_leg_knee:
+ darwin/right_leg_ankle_pitch:
+ darwin/right_leg_ankle_roll:
{}
- darwin/right_front_left_ir_link:
- {}
- darwin/right_front_right_ir_link:
- {}
- darwin/right_shoulder:
- darwin/right_arm_high:
- darwin/right_arm_low:
- {}
+ darwin/right_shoulder:
+ darwin/right_arm_high:
+ darwin/right_arm_low:
+ {}
+ obstacle_base_fence_link:
+ {}
Update Interval: 0
Value: true
- Alpha: 1
@@ -448,12 +458,112 @@ Visualization Manager:
Enabled: true
Links:
All Links Enabled: true
+ E125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E225:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ E75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
+ N125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ N75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ S75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W125:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W175:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W225:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W25:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ W75:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ obstacle_base_fence_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
+ obstacle_base_link:
+ Alpha: 1
+ Show Axes: false
+ Show Trail: false
+ Value: true
Name: Environment
- Robot Description: /charge_env/robot_description
+ Robot Description: /obstacles_env_empty/robot_description
TF Prefix: ""
Update Interval: 0
Value: true
@@ -462,7 +572,7 @@ Visualization Manager:
Global Options:
Background Color: 48; 48; 48
Default Light: true
- Fixed Frame: darwin/base_footprint
+ Fixed Frame: obstacle_base_link
Frame Rate: 30
Name: root
Tools:
@@ -483,25 +593,25 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 2.83055544
+ Distance: 11.0277748
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
- X: 0.174162328
- Y: -0.0848779902
- Z: -0.0313329101
+ X: 1.99065769
+ Y: 0.666042745
+ Z: 1.47241056
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
- Pitch: 0.130199939
+ Pitch: 0.325200558
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
- Yaw: 3.13725877
+ Yaw: 2.7204318
Saved: ~
Window Geometry:
Displays:
diff --git a/darwin_description/launch/darwin_sim.launch b/darwin_description/launch/darwin_sim.launch
index 113ed73c99241ac93bc62735cb060d16cc266ff4..cc04f7630493d0bdd9e163335e525765dc509e70 100644
--- a/darwin_description/launch/darwin_sim.launch
+++ b/darwin_description/launch/darwin_sim.launch
@@ -10,5 +10,10 @@
<arg name="robot" value="$(arg robot)" />
</include>
+ <include file="$(find darwin_driver)/launch/darwin.launch">
+ <arg name="device" value="/dev/pts/6"/>
+ <arg name="simulate" value="True"/>
+ </include>
+
</launch>
diff --git a/darwin_description/launch/charge_env.launch b/darwin_description/launch/environment.launch
similarity index 71%
rename from darwin_description/launch/charge_env.launch
rename to darwin_description/launch/environment.launch
index 85eb98ed996d2ebdeb23357c7d98c193f2165ba9..d13bf0e8fd0c42a2efab784c223ab6f8a668de64 100644
--- a/darwin_description/launch/charge_env.launch
+++ b/darwin_description/launch/environment.launch
@@ -4,12 +4,12 @@
<group ns="$(arg environment)">
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
- command="$(find xacro)/xacro --inorder '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" />
+ command="$(find xacro)/xacro --inorder '$(find bioloid_description)/urdf/ceabot/$(arg environment).xacro'" />
-<!-- <node pkg="robot_state_publisher" type="state_publisher" name="env_tf_broadcaster">
+ <node pkg="robot_state_publisher" type="state_publisher" name="env_tf_broadcaster">
<param name="tf_prefix" type="string" value="/$(arg environment)"/>
<param name="publish_frequency" type="double" value="100.0"/>
- </node>-->
+ </node>
<node name="spawn_charge_station" pkg="gazebo_ros" type="spawn_model" args="-param /$(arg environment)/robot_description -urdf -model $(arg environment) -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
</group>
diff --git a/darwin_description/urdf/darwin.gazebo b/darwin_description/urdf/darwin.gazebo
index bdcb8b7cf106bff7862a080debeed23cedf62657..a69bbf80da23ec4b309037a19b58a8632b4fccff 100644
--- a/darwin_description/urdf/darwin.gazebo
+++ b/darwin_description/urdf/darwin.gazebo
@@ -357,6 +357,8 @@
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/darwin</robotNamespace>
+ <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
+ <legacyModeNS>true</legacyModeNS>
<controlPeriod>0.0078</controlPeriod>
</plugin>
</gazebo>
diff --git a/darwin_driver/cfg/DarwinDriver.cfg b/darwin_driver/cfg/DarwinDriver.cfg
index 57d60b368d72e4ba3723cbe13934796456c27367..8c4a562b2437b6547bdc8c4501398fec3018f09f 100755
--- a/darwin_driver/cfg/DarwinDriver.cfg
+++ b/darwin_driver/cfg/DarwinDriver.cfg
@@ -43,6 +43,7 @@ gen = ParameterGenerator()
gen.add("darwin_serial", str_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial device serial number", "A4008atn")
gen.add("darwin_id", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin identifier", 2, 0, 254)
gen.add("darwin_baudrate", int_t, SensorLevels.RECONFIGURE_STOP, "Darwin serial baudrate", 1000000, 9600, 3000000)
+gen.add("simulate", bool_t, SensorLevels.RECONFIGURE_STOP, "Use driver in simualtion mode", False)
gen.add("cmd_vel_timeout", double_t, SensorLevels.RECONFIGURE_STOP, "Maximum time between cmd_vel messgaes",1.0, 0.1, 5)
exit(gen.generate(PACKAGE, "DarwinDriverDriver", "DarwinDriver"))
diff --git a/darwin_driver/launch/darwin.launch b/darwin_driver/launch/darwin.launch
index fa87189cf786465ab79cc17e4b1684616e1213e3..531df83bc9781424e122068591ec2746efe3a5eb 100644
--- a/darwin_driver/launch/darwin.launch
+++ b/darwin_driver/launch/darwin.launch
@@ -1,11 +1,16 @@
<launch>
+
+ <arg name="device" default="/dev/pts/22"/>
+ <arg name="simulate" default="False"/>
+
<node name="darwin"
pkg="darwin_driver"
type="darwin_driver"
output="screen">
- <param name="darwin_serial" value="A603LOBS"/>
+ <param name="darwin_serial" value="$(arg device)"/>
<param name="darwin_baudrate" value="1000000"/>
<param name="darwin_id" value="2"/>
+ <param name="simulate" value="$(arg simulate)"/>
</node>
</launch>
diff --git a/darwin_driver/src/darwin_driver.cpp b/darwin_driver/src/darwin_driver.cpp
index c23b0f72923bc7db2730177a2f299c9efa716ebe..f5437902fdba6b9dda3414c2a8e426723ac58821 100644
--- a/darwin_driver/src/darwin_driver.cpp
+++ b/darwin_driver/src/darwin_driver.cpp
@@ -16,7 +16,11 @@ bool DarwinDriver::openDriver(void)
delete this->darwin;
this->darwin=NULL;
}
- this->darwin=new CDarwinRobot("Darwin",this->config_.darwin_serial,this->config_.darwin_baudrate,this->config_.darwin_id);
+ if(this->config_.simulate)
+ std::cout << "using simulation mode" << std::endl;
+ else
+ std::cout << "using real mode" << std::endl;
+ this->darwin=new CDarwinRobot("Darwin",this->config_.darwin_serial,this->config_.darwin_baudrate,this->config_.darwin_id,this->config_.simulate);
// get the current walk parameters
this->walk_params.x_offset=this->darwin->walk_get_x_offset();
this->walk_params.y_offset=this->darwin->walk_get_y_offset();
diff --git a/darwin_nav/launch/darwin_global_loc_sim.launch b/darwin_nav/launch/darwin_global_loc_sim.launch
index c8581e15dab444f7ffea7304a5637a40da647730..fc3a97d82ae5cc20bf916a0a59109e5b2dcffa1b 100644
--- a/darwin_nav/launch/darwin_global_loc_sim.launch
+++ b/darwin_nav/launch/darwin_global_loc_sim.launch
@@ -7,7 +7,7 @@
output="screen">
<param name="qr_x" value="0.14"/>
<param name="qr_y" value="0.14"/>
- <param name="camera_frame" value="/darwin/darwin_cam_link"/>
+ <param name="camera_frame" value="/darwin/darwin_cam_optical_link"/>
<remap from="~/camera/image_raw" to="/darwin/camera/image_raw"/>
<remap from="~/camera/camera_info" to="/darwin/camera/camera_info"/>
</node>
@@ -16,11 +16,11 @@
name="qr_global_loc"
type="qr_global_loc"
output="screen">
- <param name="qr_map_param" value="/charge_env/robot_description"/>
+ <param name="qr_map_param" value="/obstacles_env_empty/robot_description"/>
<param name="tf_prefix" value="/darwin"/>
<param name="publish_tf" value="True"/>
<param name="publish_rate" value="20"/>
- <remap from="~/qr_pose" to="/darwin/qr_detector/qr_pose"/>
+ <remap from="~/qr_pose" to="/darwin/qr_pose"/>
</node>
</group>
diff --git a/darwin_nav/launch/global_loc_sim.launch b/darwin_nav/launch/global_loc_sim.launch
index 111f7912a257890172d606c1e134fa0601063d9e..5736ec4d6945be42d1dc39a2b4601a7ec2688a0b 100644
--- a/darwin_nav/launch/global_loc_sim.launch
+++ b/darwin_nav/launch/global_loc_sim.launch
@@ -1,14 +1,14 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
- <arg name="robot" default="darwin" />
+ <arg name="robot" default="darwin_cpp" />
<arg name="environment" default="obstacles_env_empty" />
<include file="$(find darwin_description)/launch/darwin_sim.launch">
<arg name="robot" value="$(arg robot)" />
</include>
- <include file="$(find bioloid_description)/launch/obstacles_env.launch">
+ <include file="$(find darwin_description)/launch/environment.launch">
<arg name="environment" value="$(arg environment)" />
</include>
@@ -21,7 +21,7 @@
</node>
<!-- launch the walk client node -->
- <node name="walk_client"
+<!-- <node name="walk_client"
pkg="walk_client"
type="walk_client"
output="screen"
@@ -49,7 +49,7 @@
to="/darwin/robot/set_walk_params"/>
<remap from="/darwin/walk_client/set_servo_modules"
to="/darwin/robot/set_servo_modules"/>
- </node>
+ </node>-->
<!-- launch dynamic reconfigure -->
<node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"