diff --git a/bioloid_control/config/bioloid_cm510_control.yaml b/bioloid_control/config/bioloid_cm510_control.yaml
index 0e1b647a50c64737cc85b37d6d07861be6ce8b02..b327a9f52a792943c44379742090ed1fdc0fa72f 100644
--- a/bioloid_control/config/bioloid_cm510_control.yaml
+++ b/bioloid_control/config/bioloid_cm510_control.yaml
@@ -11,6 +11,12 @@ bioloid:
adc4_frame: gyro_y
exp_adc1_frame: IR2_link
exp_board_id: 192
+ exp_gpio0_frame: left_front_fwd_ir_link
+ exp_gpio1_frame: left_front_dwn_ir_link
+ exp_gpio2_frame: left_lateral_dwn_ir_link
+ exp_gpio3_frame: right_front_fwd_ir_link
+ exp_gpio4_frame: right_front_dwn_ir_link
+ exp_gpio5_frame: right_lateral_dwn_ir_link
joints:
- j_shoulder_r
- j_shoulder_l
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510.h b/bioloid_controller_cm510/include/bioloid_controller_cm510.h
index 93f10c5bcc4e73d324f99f40f10f024ae0635d06..e2dfe328cbab4d4a86ecb66f98394decdba36e27 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510.h
@@ -82,6 +82,7 @@ namespace bioloid_controller_cm510
void convert_page_info(void);
void init_controller_analog_ports(ros::NodeHandle &nh);
void init_exp_board_analog_ports(ros::NodeHandle &nh);
+ void init_exp_board_gpio_ports(ros::NodeHandle &nh);
private:
typedef typename HardwareInterface::ResourceHandleType JointHandle;
@@ -110,6 +111,26 @@ namespace bioloid_controller_cm510
std::string exp_adc6_frame;
std::string exp_adc7_frame;
std::string exp_adc8_frame;
+ /* frame associated to each of the GPIO of the expansion board */
+ std::string exp_gpio0_frame;
+ std::string exp_gpio1_frame;
+ std::string exp_gpio2_frame;
+ std::string exp_gpio3_frame;
+ std::string exp_gpio4_frame;
+ std::string exp_gpio5_frame;
+ std::string exp_gpio6_frame;
+ std::string exp_gpio7_frame;
+ std::string exp_gpio8_frame;
+ std::string exp_gpio9_frame;
+ std::string exp_gpio10_frame;
+ std::string exp_gpio11_frame;
+ std::string exp_gpio12_frame;
+ std::string exp_gpio13_frame;
+ std::string exp_gpio14_frame;
+ std::string exp_gpio15_frame;
+ std::string exp_gpio16_frame;
+ std::string exp_gpio17_frame;
+ std::string exp_gpio18_frame;
/* motion file handling */
CRobotisMtn motions;
std::string motions_file_name;
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 7ccba10f1da5fed0adedf775f7ee002529df07c1..6d46dfe1e3283babaef725a9ee025bb32895816c 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -57,6 +57,7 @@ extern "C" {
extern int exp_board_id;
extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
+extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
namespace bioloid_controller_cm510
{
@@ -244,6 +245,7 @@ namespace bioloid_controller_cm510
/* read the configuration */
this->init_controller_analog_ports(controller_nh);
this->init_exp_board_analog_ports(controller_nh);
+ this->init_exp_board_gpio_ports(controller_nh);
/* read the expansion board identifier */
exp_board_id=-1;
controller_nh_.getParam("exp_board_id", exp_board_id);
@@ -378,7 +380,15 @@ namespace bioloid_controller_cm510
unsigned int i=0;
unsigned short int value;
double B,C,voltage;
-
+ std::string frame_name;
+ unsigned char threshold;
+
+ if(msg->header.frame_id.find("/")==std::string::npos)
+ frame_name=msg->header.frame_id;
+ else
+ frame_name=msg->header.frame_id.substr(msg->header.frame_id.find("/")+1);
+
+ /* handle the controller analog ports */
for(i=0;i<NUM_ADC;i++)
{
const double Vmax=3.3,Vmin=0.5;
@@ -387,19 +397,20 @@ namespace bioloid_controller_cm510
C=(100.0-Vmax*msg->min_range*B)/Vmax;
voltage=100.0/(B*msg->range+C);// apply the approximation of the transfer function of the sensor
value=voltage*1023.0/Vmax;// transform it to a digital value
- if(msg->header.frame_id==this->adc1_frame)
+ if(frame_name==this->adc1_frame)
adc_values[ADC_PORT_1]=value;
- else if(msg->header.frame_id==this->adc2_frame)
+ else if(frame_name==this->adc2_frame)
adc_values[ADC_PORT_2]=value;
- else if(msg->header.frame_id==this->adc3_frame)
+ else if(frame_name==this->adc3_frame)
adc_values[ADC_PORT_3]=value;
- else if(msg->header.frame_id==this->adc4_frame)
+ else if(frame_name==this->adc4_frame)
adc_values[ADC_PORT_4]=value;
- else if(msg->header.frame_id==this->adc5_frame)
+ else if(frame_name==this->adc5_frame)
adc_values[ADC_PORT_5]=value;
- else if(msg->header.frame_id==this->adc6_frame)
+ else if(frame_name==this->adc6_frame)
adc_values[ADC_PORT_6]=value;
}
+ /* handle the expansion board analog ports */
if(exp_board_id!=-1)
{
const double Vmax=5.0,Vmin=0.5;
@@ -410,24 +421,70 @@ namespace bioloid_controller_cm510
value=voltage*1023.0/Vmax;// transform it to a digital value
for(i=0;i<EXP_NUM_ADC;i++)
{
- if(msg->header.frame_id==this->exp_adc1_frame)
+ if(frame_name==this->exp_adc1_frame)
exp_adc_values[0]=value;
- else if(msg->header.frame_id==this->exp_adc2_frame)
+ else if(frame_name==this->exp_adc2_frame)
exp_adc_values[1]=value;
- else if(msg->header.frame_id==this->exp_adc3_frame)
+ else if(frame_name==this->exp_adc3_frame)
exp_adc_values[2]=value;
- else if(msg->header.frame_id==this->exp_adc4_frame)
+ else if(frame_name==this->exp_adc4_frame)
exp_adc_values[3]=value;
- else if(msg->header.frame_id==this->exp_adc5_frame)
+ else if(frame_name==this->exp_adc5_frame)
exp_adc_values[4]=value;
- else if(msg->header.frame_id==this->exp_adc6_frame)
+ else if(frame_name==this->exp_adc6_frame)
exp_adc_values[5]=value;
- else if(msg->header.frame_id==this->exp_adc7_frame)
+ else if(frame_name==this->exp_adc7_frame)
exp_adc_values[6]=value;
- else if(msg->header.frame_id==this->exp_adc8_frame)
+ else if(frame_name==this->exp_adc8_frame)
exp_adc_values[7]=value;
}
}
+ /* handle the feet sensors */
+ for(i=0;i<EXP_NUM_GPIO;i++)
+ {
+ if(msg->range<(msg->max_range/2.0))
+ threshold=0x01;
+ else
+ threshold=0x00;
+ if(frame_name==this->exp_gpio0_frame)
+ exp_gpio_values[0]=threshold;
+ else if(frame_name==this->exp_gpio1_frame)
+ exp_gpio_values[1]=threshold;
+ else if(frame_name==this->exp_gpio2_frame)
+ exp_gpio_values[2]=threshold;
+ else if(frame_name==this->exp_gpio3_frame)
+ exp_gpio_values[3]=threshold;
+ else if(frame_name==this->exp_gpio4_frame)
+ exp_gpio_values[4]=threshold;
+ else if(frame_name==this->exp_gpio5_frame)
+ exp_gpio_values[5]=threshold;
+ else if(frame_name==this->exp_gpio6_frame)
+ exp_gpio_values[6]=threshold;
+ else if(frame_name==this->exp_gpio7_frame)
+ exp_gpio_values[7]=threshold;
+ else if(frame_name==this->exp_gpio8_frame)
+ exp_gpio_values[8]=threshold;
+ else if(frame_name==this->exp_gpio9_frame)
+ exp_gpio_values[9]=threshold;
+ else if(frame_name==this->exp_gpio10_frame)
+ exp_gpio_values[10]=threshold;
+ else if(frame_name==this->exp_gpio11_frame)
+ exp_gpio_values[11]=threshold;
+ else if(frame_name==this->exp_gpio12_frame)
+ exp_gpio_values[12]=threshold;
+ else if(frame_name==this->exp_gpio13_frame)
+ exp_gpio_values[13]=threshold;
+ else if(frame_name==this->exp_gpio14_frame)
+ exp_gpio_values[14]=threshold;
+ else if(frame_name==this->exp_gpio15_frame)
+ exp_gpio_values[15]=threshold;
+ else if(frame_name==this->exp_gpio16_frame)
+ exp_gpio_values[16]=threshold;
+ else if(frame_name==this->exp_gpio17_frame)
+ exp_gpio_values[17]=threshold;
+ else if(frame_name==this->exp_gpio18_frame)
+ exp_gpio_values[18]=threshold;
+ }
}
template <class HardwareInterface>
@@ -584,6 +641,67 @@ namespace bioloid_controller_cm510
nh.getParam("exp_adc8_frame", this->exp_adc8_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC8 associated to sensor: " << this->exp_adc8_frame);
}
+ template <class HardwareInterface>
+ void BioloidControllerCM510<HardwareInterface>::init_exp_board_gpio_ports(ros::NodeHandle &nh)
+ {
+ this->exp_gpio0_frame="none";
+ nh.getParam("exp_gpio0_frame", this->exp_gpio0_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO0 associated to sensor: " << this->exp_gpio0_frame);
+ this->exp_gpio1_frame="none";
+ nh.getParam("exp_gpio1_frame", this->exp_gpio1_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO1 associated to sensor: " << this->exp_gpio1_frame);
+ this->exp_gpio2_frame="none";
+ nh.getParam("exp_gpio2_frame", this->exp_gpio2_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO2 associated to sensor: " << this->exp_gpio2_frame);
+ this->exp_gpio3_frame="none";
+ nh.getParam("exp_gpio3_frame", this->exp_gpio3_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO3 associated to sensor: " << this->exp_gpio3_frame);
+ this->exp_gpio4_frame="none";
+ nh.getParam("exp_gpio4_frame", this->exp_gpio4_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO4 associated to sensor: " << this->exp_gpio4_frame);
+ this->exp_gpio5_frame="none";
+ nh.getParam("exp_gpio5_frame", this->exp_gpio5_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO5 associated to sensor: " << this->exp_gpio5_frame);
+ this->exp_gpio6_frame="none";
+ nh.getParam("exp_gpio6_frame", this->exp_gpio6_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO6 associated to sensor: " << this->exp_gpio6_frame);
+ this->exp_gpio7_frame="none";
+ nh.getParam("exp_gpio7_frame", this->exp_gpio7_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO7 associated to sensor: " << this->exp_gpio7_frame);
+ this->exp_gpio8_frame="none";
+ nh.getParam("exp_gpio8_frame", this->exp_gpio8_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO8 associated to sensor: " << this->exp_gpio8_frame);
+ this->exp_gpio9_frame="none";
+ nh.getParam("exp_gpio9_frame", this->exp_gpio9_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO9 associated to sensor: " << this->exp_gpio9_frame);
+ this->exp_gpio10_frame="none";
+ nh.getParam("exp_gpio10_frame", this->exp_gpio10_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO10 associated to sensor: " << this->exp_gpio10_frame);
+ this->exp_gpio11_frame="none";
+ nh.getParam("exp_gpio11_frame", this->exp_gpio11_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO11 associated to sensor: " << this->exp_gpio11_frame);
+ this->exp_gpio12_frame="none";
+ nh.getParam("exp_gpio12_frame", this->exp_gpio12_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO12 associated to sensor: " << this->exp_gpio12_frame);
+ this->exp_gpio13_frame="none";
+ nh.getParam("exp_gpio13_frame", this->exp_gpio13_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO13 associated to sensor: " << this->exp_gpio13_frame);
+ this->exp_gpio14_frame="none";
+ nh.getParam("exp_gpio14_frame", this->exp_gpio14_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO14 associated to sensor: " << this->exp_gpio14_frame);
+ this->exp_gpio15_frame="none";
+ nh.getParam("exp_gpio15_frame", this->exp_gpio15_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO15 associated to sensor: " << this->exp_gpio15_frame);
+ this->exp_gpio16_frame="none";
+ nh.getParam("exp_gpio16_frame", this->exp_gpio16_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO16 associated to sensor: " << this->exp_gpio16_frame);
+ this->exp_gpio17_frame="none";
+ nh.getParam("exp_gpio17_frame", this->exp_gpio17_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO17 associated to sensor: " << this->exp_gpio17_frame);
+ this->exp_gpio18_frame="none";
+ nh.getParam("exp_gpio18_frame", this->exp_gpio18_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO18 associated to sensor: " << this->exp_gpio18_frame);
+ }
} // namespace
#endif // header guard
diff --git a/bioloid_controller_cm510/include/sim/exp_board_sim.h b/bioloid_controller_cm510/include/sim/exp_board_sim.h
index d654a278c02e1728ab28bce064498f31257e84da..45a262484c7485e5df83e291cab82eaddd91b7c7 100644
--- a/bioloid_controller_cm510/include/sim/exp_board_sim.h
+++ b/bioloid_controller_cm510/include/sim/exp_board_sim.h
@@ -8,6 +8,7 @@ extern "C" {
#include <avr/io.h>
#define EXP_NUM_ADC 8
+#define EXP_NUM_GPIO 19
void exp_board_sim_init(void);
void exp_board_sim_loop(void);
diff --git a/bioloid_controller_cm510/src/sim/adc.c b/bioloid_controller_cm510/src/sim/adc.c
index 29200fdc9af419b2d9bf0133df23b96d57b07f26..c5562c1c175c822ac6bd81f14475c0c9f265a4b1 100644
--- a/bioloid_controller_cm510/src/sim/adc.c
+++ b/bioloid_controller_cm510/src/sim/adc.c
@@ -58,10 +58,10 @@ void init_adc(void)
for(i=0;i<NUM_ADC;i++)
{
- adc_values[i]=512;
- adc_avg_values[i]=512;
+ adc_values[i]=256;
+ adc_avg_values[i]=256;
for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
- adc_ch_data[i][j]=512;
+ adc_ch_data[i][j]=256;
}
adc_current_sample=0;
}
diff --git a/bioloid_controller_cm510/src/sim/dynamixel_master.c b/bioloid_controller_cm510/src/sim/dynamixel_master.c
index 94982057f68d76b83c5bb84852879b49bc654356..c5dcac98dff2aec4390c69666ad619345c165232 100644
--- a/bioloid_controller_cm510/src/sim/dynamixel_master.c
+++ b/bioloid_controller_cm510/src/sim/dynamixel_master.c
@@ -14,6 +14,7 @@ extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_adc_avg_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
extern uint16_t exp_compass_avg_value;
+extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
/* private variables */
uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
@@ -193,6 +194,44 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
*data=exp_compass_value;
else if(address==COMPASS_AVG_HEADING_VAL_L)
*data=exp_compass_avg_value;
+ else if(address==GPIO0_data)
+ *data=exp_gpio_values[0];
+ else if(address==GPIO1_data)
+ *data=exp_gpio_values[1];
+ else if(address==GPIO2_data)
+ *data=exp_gpio_values[2];
+ else if(address==GPIO3_data)
+ *data=exp_gpio_values[3];
+ else if(address==GPIO4_data)
+ *data=exp_gpio_values[4];
+ else if(address==GPIO5_data)
+ *data=exp_gpio_values[5];
+ else if(address==GPIO6_data)
+ *data=exp_gpio_values[6];
+ else if(address==GPIO7_data)
+ *data=exp_gpio_values[7];
+ else if(address==GPIO8_data)
+ *data=exp_gpio_values[8];
+ else if(address==GPIO9_data)
+ *data=exp_gpio_values[9];
+ else if(address==GPIO10_data)
+ *data=exp_gpio_values[10];
+ else if(address==GPIO11_data)
+ *data=exp_gpio_values[11];
+ else if(address==GPIO12_data)
+ *data=exp_gpio_values[12];
+ else if(address==GPIO13_data)
+ *data=exp_gpio_values[13];
+ else if(address==GPIO14_data)
+ *data=exp_gpio_values[14];
+ else if(address==GPIO15_data)
+ *data=exp_gpio_values[15];
+ else if(address==GPIO16_data)
+ *data=exp_gpio_values[16];
+ else if(address==GPIO17_data)
+ *data=exp_gpio_values[17];
+ else if(address==GPIO18_data)
+ *data=exp_gpio_values[18];
}
return DYN_SUCCESS;
diff --git a/bioloid_controller_cm510/src/sim/exp_board_sim.c b/bioloid_controller_cm510/src/sim/exp_board_sim.c
index 7527156f4b3d9f9174e925d8dfc1bef1189ce540..7959fb412eb47651052485a02435fec6105dc380 100644
--- a/bioloid_controller_cm510/src/sim/exp_board_sim.c
+++ b/bioloid_controller_cm510/src/sim/exp_board_sim.c
@@ -13,6 +13,8 @@ uint16_t exp_compass_value;
uint16_t exp_compass_avg_value;
uint8_t exp_compass_current_sample;
+uint8_t exp_gpio_values[EXP_NUM_GPIO];
+
void exp_board_compass_avrg(uint16_t heading)
{
static unsigned long int new_average,first_value;
@@ -62,6 +64,9 @@ void exp_board_sim_init(void)
exp_compass_value=0;
exp_compass_avg_value=0;
exp_compass_current_sample=0;
+ /* initialize the expansion board gpio data */
+ for(i=0;i<EXP_NUM_GPIO;i++)
+ exp_gpio_values[i]=0x00;
}
void exp_board_sim_loop(void)
diff --git a/bioloid_description/urdf/bioloid_ceabot.xacro b/bioloid_description/urdf/bioloid_ceabot.xacro
index 2c1d9250d50aeead5ddf97ae3af2eaee352f5d76..82db14f1ed39e5e1777e3688b18b6a8cc7e229b3 100755
--- a/bioloid_description/urdf/bioloid_ceabot.xacro
+++ b/bioloid_description/urdf/bioloid_ceabot.xacro
@@ -4,12 +4,12 @@
<xacro:include filename="$(find bioloid_description)/urdf/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/feet_ir.xacro" />
- <xacro:sharp_ir name="IR1" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
+ <xacro:sharp_ir name="IR1" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
- <xacro:sharp_ir name="IR2" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
+ <xacro:sharp_ir name="IR2" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 -0.05 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
- <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="100" range="0.005"/>
+ <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>
</robot>
diff --git a/bioloid_description/urdf/cny70_ir.gazebo b/bioloid_description/urdf/cny70_ir.gazebo
index a0b2b85226c528dc74460d06aa32162150a75dc4..ed1b1f68bc35457fda2ad3346c01f6068727f8fc 100644
--- a/bioloid_description/urdf/cny70_ir.gazebo
+++ b/bioloid_description/urdf/cny70_ir.gazebo
@@ -19,7 +19,7 @@
<sensor type="ray" name="${name}">
<always_on>true</always_on>
<update_rate>${update_rate}</update_rate>
- <visualize>false</visualize>
+ <visualize>true</visualize>
<ray>
<scan>
<horizontal>
@@ -42,11 +42,11 @@
</range>
</ray>
<plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
- <frameName>${name}_ir_link</frameName>
+ <frameName>/bioloid/${name}_ir_link</frameName>
<topicName>/bioloid/sensors/range</topicName>
<radiation>infrared</radiation>
<fov>${fov}</fov>
- <gaussianNoise>0.01</gaussianNoise>
+ <gaussianNoise>0.0005</gaussianNoise>
<updateRate>${update_rate}</updateRate>
</plugin>
</sensor>
diff --git a/bioloid_description/urdf/cny70_ir.xacro b/bioloid_description/urdf/cny70_ir.xacro
index 1798773abe0caa84408e4adfc69b448a585a00b7..a566bef1c022be53e944cf52100f9bdc77084960 100644
--- a/bioloid_description/urdf/cny70_ir.xacro
+++ b/bioloid_description/urdf/cny70_ir.xacro
@@ -21,7 +21,7 @@
<material name="black"/>
</visual>
<inertial>
- <mass value="0.0007" />
+ <mass value="0.0007"/>
<origin xyz="-0.00307736 0.0000000 0.00000000" rpy="0 0 0"/>
<inertia ixx="0.000000001" ixy="0.00000000" ixz="0.000000" iyy="0.000000001" iyz="0.00000000" izz="0.000000001" />
</inertial>
diff --git a/bioloid_description/urdf/feet_ir.xacro b/bioloid_description/urdf/feet_ir.xacro
index 04dc1afc9242775b041d2388bf1cf9c021bfecf7..93ca7bc818300af66643425bfb5576a9f4d4a0cd 100644
--- a/bioloid_description/urdf/feet_ir.xacro
+++ b/bioloid_description/urdf/feet_ir.xacro
@@ -34,15 +34,15 @@
</joint>
<!-- sensor links -->
- <xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
</xacro:cny70_ir>
- <xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
- <origin xyz="0.0225 -0.012 0.038" rpy="0 0 -1.5707" />
+ <xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
- <xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
@@ -74,15 +74,15 @@
</joint>
<!-- sensor links -->
- <xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="-0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
</xacro:cny70_ir>
- <xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
- <origin xyz="-0.0225 -0.012 0.038" rpy="0 0 -1.5707" />
+ <xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
- <xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="${range}" max_range="${range}">
+ <xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
<origin xyz="-0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
</xacro:cny70_ir>
diff --git a/bioloid_description/urdf/sharp_ir.gazebo b/bioloid_description/urdf/sharp_ir.gazebo
index b068a9344a098c50387c15cb88019c0de9e2ab2f..117d9f436704c21a813ccbb6c66ef5cd104d48fd 100644
--- a/bioloid_description/urdf/sharp_ir.gazebo
+++ b/bioloid_description/urdf/sharp_ir.gazebo
@@ -42,7 +42,7 @@
</range>
</ray>
<plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
- <frameName>${name}_link</frameName>
+ <frameName>/bioloid/${name}_link</frameName>
<topicName>/bioloid/sensors/range</topicName>
<radiation>infrared</radiation>
<fov>${fov}</fov>